[PATCH 08/10] UefiCpuPkg/MtrrLib: Use a better algorithm to calculate MTRR

[Ruiyu Ni <ruiyu.ni@intel.com>]

The new algorithm finds out the more optimal MTRR solution for
current memory type settings.
Compare against the original algorithm, the new one guarantees
to find the correct MTRR solution, but doesn't guarantee to
find the most optimal MTRR solution.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Ruiyu Ni <ruiyu.ni@intel.com>
Cc: Jeff Fan <jeff.fan@intel.com>
---
 UefiCpuPkg/Library/MtrrLib/MtrrLib.c | 1466 +++++++++++++++++++++-------------
 1 file changed, 906 insertions(+), 560 deletions(-)

diff --git a/UefiCpuPkg/Library/MtrrLib/MtrrLib.c b/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
index bcc2df5..34e6ad6 100644
--- a/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
+++ b/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
@@ -30,6 +30,7 @@
 #define OR_SEED      0x0101010101010101ull
 #define CLEAR_SEED   0xFFFFFFFFFFFFFFFFull
 
+#define MTRR_LIB_ASSERT_ALIGNED(B, L) ASSERT ((B & ~(L - 1)) == B);
 //
 // Context to save and restore when MTRRs are programmed
 //
@@ -38,6 +39,12 @@ typedef struct {
   BOOLEAN  InterruptState;
 } MTRR_CONTEXT;
 
+typedef struct {
+  UINT64                 BaseAddress;
+  UINT64                 Length;
+  MTRR_MEMORY_CACHE_TYPE Type;
+} MEMORY_RANGE;
+
 //
 // This table defines the offset, base and length of the fixed MTRRs
 //
@@ -642,361 +649,309 @@ MtrrGetMemoryAttributeInVariableMtrr (
            );
 }
 
-
 /**
-  Checks overlap between given memory range and MTRRs.
-
-  @param[in]  FirmwareVariableMtrrCount  The number of variable MTRRs available
-                                         to firmware.
-  @param[in]  Start                      The start address of memory range.
-  @param[in]  End                        The end address of memory range.
-  @param[in]  VariableMtrr               The array to shadow variable MTRRs content
+  Return the least alignment of address.
 
-  @retval TRUE             Overlap exists.
-  @retval FALSE            No overlap.
+  @param Address    The address to return the alignment.
+  @param Alignment0 The alignment to return when Address is 0.
 
+  @return The least alignment of the Address.
 **/
-BOOLEAN
-CheckMemoryAttributeOverlap (
-  IN UINTN             FirmwareVariableMtrrCount,
-  IN PHYSICAL_ADDRESS  Start,
-  IN PHYSICAL_ADDRESS  End,
-  IN VARIABLE_MTRR     *VariableMtrr
-  )
+UINT64
+MtrrLibLeastAlignment (
+  UINT64    Address,
+  UINT64    Alignment0
+)
 {
-  UINT32  Index;
-
-  for (Index = 0; Index < FirmwareVariableMtrrCount; Index++) {
-    if (
-         VariableMtrr[Index].Valid &&
-         !(
-           (Start > (VariableMtrr[Index].BaseAddress +
-                     VariableMtrr[Index].Length - 1)
-           ) ||
-           (End < VariableMtrr[Index].BaseAddress)
-         )
-       ) {
-      return TRUE;
-    }
+  if (Address == 0) {
+    return Alignment0;
   }
 
-  return FALSE;
-}
-
-
-/**
-  Marks a variable MTRR as non-valid.
-
-  @param[in]   Index         The index of the array VariableMtrr to be invalidated
-  @param[in]   VariableMtrr  The array to shadow variable MTRRs content
-  @param[out]  UsedMtrr      The number of MTRRs which has already been used
-
-**/
-VOID
-InvalidateShadowMtrr (
-  IN   UINTN              Index,
-  IN   VARIABLE_MTRR      *VariableMtrr,
-  OUT  UINT32             *UsedMtrr
-  )
-{
-  VariableMtrr[Index].Valid = FALSE;
-  *UsedMtrr = *UsedMtrr - 1;
+  return LShiftU64 (1, (UINTN) LowBitSet64 (Address));
 }
 
-
 /**
-  Combines memory attributes.
+  Return the number of required variable MTRRs to positively cover the
+  specified range.
 
-  If overlap exists between given memory range and MTRRs, try to combine them.
-
-  @param[in]       FirmwareVariableMtrrCount  The number of variable MTRRs
-                                              available to firmware.
-  @param[in]       Attributes                 The memory type to set.
-  @param[in, out]  Base                       The base address of memory range.
-  @param[in, out]  Length                     The length of memory range.
-  @param[in]       VariableMtrr               The array to shadow variable MTRRs content
-  @param[in, out]  UsedMtrr                   The number of MTRRs which has already been used
-  @param[out]      OverwriteExistingMtrr      Returns whether an existing MTRR was used
-
-  @retval EFI_SUCCESS            Memory region successfully combined.
-  @retval EFI_ACCESS_DENIED      Memory region cannot be combined.
+  @param BaseAddress Base address of the range.
+  @param Length      Length of the range.
+  @param Alignment0  Alignment of 0.
 
+  @return The number of the required variable MTRRs.
 **/
-RETURN_STATUS
-CombineMemoryAttribute (
-  IN     UINT32             FirmwareVariableMtrrCount,
-  IN     UINT64             Attributes,
-  IN OUT UINT64             *Base,
-  IN OUT UINT64             *Length,
-  IN     VARIABLE_MTRR      *VariableMtrr,
-  IN OUT UINT32             *UsedMtrr,
-  OUT    BOOLEAN            *OverwriteExistingMtrr
-  )
+UINT32
+MtrrLibGetPositiveMtrrNumber (
+  IN UINT64      BaseAddress,
+  IN UINT64      Length,
+  IN UINT64      Alignment0
+)
 {
-  UINT32  Index;
-  UINT64  CombineStart;
-  UINT64  CombineEnd;
-  UINT64  MtrrEnd;
-  UINT64  EndAddress;
-  BOOLEAN CoveredByExistingMtrr;
-
-  *OverwriteExistingMtrr = FALSE;
-  CoveredByExistingMtrr = FALSE;
-  EndAddress = *Base +*Length - 1;
-
-  for (Index = 0; Index < FirmwareVariableMtrrCount; Index++) {
+  UINT64         SubLength;
+  UINT32         MtrrNumber;
+  BOOLEAN        UseLeastAlignment;
 
-    MtrrEnd = VariableMtrr[Index].BaseAddress + VariableMtrr[Index].Length - 1;
-    if (
-         !VariableMtrr[Index].Valid ||
-         (
-           *Base > (MtrrEnd) ||
-           (EndAddress < VariableMtrr[Index].BaseAddress)
-         )
-       ) {
-      continue;
-    }
+  UseLeastAlignment = TRUE;
 
-    //
-    // Combine same attribute MTRR range
-    //
-    if (Attributes == VariableMtrr[Index].Type) {
-      //
-      // if the MTRR range contain the request range, set a flag, then continue to
-      // invalidate any MTRR of the same request range with higher priority cache type.
-      //
-      if (VariableMtrr[Index].BaseAddress <= *Base && MtrrEnd >= EndAddress) {
-        CoveredByExistingMtrr = TRUE;
-        continue;
-      }
-      //
-      // invalid this MTRR, and program the combine range
-      //
-      CombineStart  =
-        (*Base) < VariableMtrr[Index].BaseAddress ?
-          (*Base) :
-          VariableMtrr[Index].BaseAddress;
-      CombineEnd    = EndAddress > MtrrEnd ? EndAddress : MtrrEnd;
+  //
+  // Calculate the alignment of the base address.
+  //
+  for (MtrrNumber = 0; Length != 0; MtrrNumber++) {
+    if (UseLeastAlignment) {
+      SubLength = MtrrLibLeastAlignment (BaseAddress, Alignment0);
 
-      //
-      // Record the MTRR usage status in VariableMtrr array.
-      //
-      InvalidateShadowMtrr (Index, VariableMtrr, UsedMtrr);
-      *Base       = CombineStart;
-      *Length     = CombineEnd - CombineStart + 1;
-      EndAddress  = CombineEnd;
-      *OverwriteExistingMtrr = TRUE;
-      continue;
-    } else {
-      //
-      // The cache type is different, but the range is covered by one MTRR
-      //
-      if (VariableMtrr[Index].BaseAddress == *Base && MtrrEnd == EndAddress) {
-        InvalidateShadowMtrr (Index, VariableMtrr, UsedMtrr);
-        continue;
+      if (SubLength > Length) {
+        //
+        // Set a flag when remaining length is too small
+        //  so that MtrrLibLeastAlignment() is not called in following loops.
+        //
+        UseLeastAlignment = FALSE;
       }
-
     }
 
-    if ((Attributes== MTRR_CACHE_WRITE_THROUGH &&
-         VariableMtrr[Index].Type == MTRR_CACHE_WRITE_BACK) ||
-        (Attributes == MTRR_CACHE_WRITE_BACK &&
-         VariableMtrr[Index].Type == MTRR_CACHE_WRITE_THROUGH) ||
-        (Attributes == MTRR_CACHE_UNCACHEABLE) ||
-        (VariableMtrr[Index].Type == MTRR_CACHE_UNCACHEABLE)
-     ) {
-      *OverwriteExistingMtrr = TRUE;
-      continue;
+    if (!UseLeastAlignment) {
+      SubLength = GetPowerOfTwo64 (Length);
     }
-    //
-    // Other type memory overlap is invalid
-    //
-    return RETURN_ACCESS_DENIED;
-  }
 
-  if (CoveredByExistingMtrr) {
-    *Length = 0;
+    BaseAddress += SubLength;
+    Length -= SubLength;
   }
 
-  return RETURN_SUCCESS;
+  return MtrrNumber;
 }
 
-
 /**
-  Calculates the maximum value which is a power of 2, but less the MemoryLength.
+  Return whether the left MTRR type precedes the right MTRR type.
 
-  @param[in]  MemoryLength        The number to pass in.
+  The MTRR type precedence rules are:
+  1. UC precedes any other type
+  2. WT precedes WB
 
-  @return The maximum value which is align to power of 2 and less the MemoryLength
+  @param Left  The left MTRR type.
+  @param Right The right MTRR type.
 
+  @retval TRUE  Left precedes Right.
+  @retval FALSE Left doesn't precede Right.
 **/
-UINT64
-Power2MaxMemory (
-  IN UINT64                     MemoryLength
-  )
+BOOLEAN
+MtrrLibTypeLeftPrecedeRight (
+  IN MTRR_MEMORY_CACHE_TYPE  Left,
+  IN MTRR_MEMORY_CACHE_TYPE  Right
+)
 {
-  UINT64  Result;
-
-  if (RShiftU64 (MemoryLength, 32) != 0) {
-    Result = LShiftU64 (
-               (UINT64) GetPowerOfTwo32 (
-                          (UINT32) RShiftU64 (MemoryLength, 32)
-                          ),
-               32
-               );
-  } else {
-    Result = (UINT64) GetPowerOfTwo32 ((UINT32) MemoryLength);
-  }
-
-  return Result;
+  return (BOOLEAN) (Left == CacheUncacheable || (Left == CacheWriteThrough && Right == CacheWriteBack));
 }
 
 
 /**
-  Determines the MTRR numbers used to program a memory range.
+  Return whether the type of the specified range can precede the specified type.
+
+  @param Ranges     Memory range array holding memory type settings for all
+                    the memory address.
+  @param RangeCount Count of memory ranges.
+  @param Type       Type to check precedence.
+  @param SubBase    Base address of the specified range.
+  @param SubLength  Length of the specified range.
+
+  @retval TRUE  The type of the specified range can precede the Type.
+  @retval FALSE The type of the specified range cannot precede the Type.
+                So the subtraction is not applicable.
+**/
+BOOLEAN
+MtrrLibSubstractable (
+  IN CONST MEMORY_RANGE     *Ranges,
+  IN UINT32                  RangeCount,
+  IN MTRR_MEMORY_CACHE_TYPE  Type,
+  IN UINT64                  SubBase,
+  IN UINT64                  SubLength
+)
+{
+  UINT32                     Index;
+  UINT64                     Length;
+  // WT > WB
+  // UC > *
+  for (Index = 0; Index < RangeCount; Index++) {
+    if (Ranges[Index].BaseAddress <= SubBase && SubBase < Ranges[Index].BaseAddress + Ranges[Index].Length) {
 
-  This function first checks the alignment of the base address.
-  If the alignment of the base address <= Length, cover the memory range
- (BaseAddress, alignment) by a MTRR, then BaseAddress += alignment and
-  Length -= alignment. Repeat the step until alignment > Length.
+      if (Ranges[Index].BaseAddress + Ranges[Index].Length >= SubBase + SubLength) {
+        return MtrrLibTypeLeftPrecedeRight (Ranges[Index].Type, Type);
 
-  Then this function determines which direction of programming the variable
-  MTRRs for the remaining length will use fewer MTRRs.
+      } else {
+        if (!MtrrLibTypeLeftPrecedeRight (Ranges[Index].Type, Type)) {
+          return FALSE;
+        }
 
-  @param[in]  BaseAddress Length of Memory to program MTRR
-  @param[in]  Length      Length of Memory to program MTRR
-  @param[in]  MtrrNumber  Pointer to the number of necessary MTRRs
+        Length = Ranges[Index].BaseAddress + Ranges[Index].Length - SubBase;
+        SubBase += Length;
+        SubLength -= Length;
+      }
+    }
+  }
 
-  @retval TRUE        Positive direction is better.
-          FALSE       Negative direction is better.
+  ASSERT (FALSE);
+  return FALSE;
+}
 
+/**
+  Return the number of required variable MTRRs to cover the specified range.
+
+  The routine considers subtraction in the both side of the range to find out
+  the most optimal solution (which uses the least MTRRs).
+
+  @param Ranges            Array holding memory type settings of all memory
+                           address.
+  @param RangeCount        Count of memory ranges.
+  @param VariableMtrr      Array holding allocated variable MTRRs.
+  @param VariableMtrrCount Count of allocated variable MTRRs.
+  @param BaseAddress       Base address of the specified range.
+  @param Length            Length of the specified range.
+  @param Type              MTRR type of the specified range.
+  @param Alignment0        Alignment of 0.
+  @param SubLeft           Return the count of left subtraction.
+  @param SubRight          Return the count of right subtraction.
+
+  @return Number of required variable MTRRs.
 **/
-BOOLEAN
-GetMtrrNumberAndDirection (
-  IN UINT64      BaseAddress,
-  IN UINT64      Length,
-  IN UINTN       *MtrrNumber
-  )
+UINT32
+MtrrLibGetMtrrNumber (
+  IN CONST MEMORY_RANGE    *Ranges,
+  IN UINT32                 RangeCount,
+  IN CONST VARIABLE_MTRR    *VariableMtrr,
+  IN UINT32                 VariableMtrrCount,
+  IN UINT64                 BaseAddress,
+  IN UINT64                 Length,
+  IN MTRR_MEMORY_CACHE_TYPE Type,
+  IN UINT64                 Alignment0,
+  OUT UINT32                *SubLeft, // subtractive from BaseAddress to get more aligned address, to save MTRR
+  OUT UINT32                *SubRight // subtractive from BaseAddress + Length, to save MTRR
+)
 {
-  UINT64  TempQword;
   UINT64  Alignment;
-  UINT32  Positive;
-  UINT32  Subtractive;
+  UINT32  LeastLeftMtrrNumber;
+  UINT32  MiddleMtrrNumber;
+  UINT32  LeastRightMtrrNumber;
+  UINT32  CurrentMtrrNumber;
+  UINT32  SubtractiveCount;
+  UINT32  SubtractiveMtrrNumber;
+  UINT32  LeastSubtractiveMtrrNumber;
+  UINT64  SubtractiveBaseAddress;
+  UINT64  SubtractiveLength;
+  UINT64  BaseAlignment;
+  UINT32  Index;
 
-  *MtrrNumber = 0;
+  *SubLeft = 0;
+  *SubRight = 0;
+  LeastSubtractiveMtrrNumber = 0;
 
+  //
+  // Get the optimal left subtraction solution.
+  //
   if (BaseAddress != 0) {
-    do {
+    //
+    // Get the MTRR number needed without left subtraction.
+    //
+    LeastLeftMtrrNumber = MtrrLibGetPositiveMtrrNumber (BaseAddress, Length, Alignment0);
+
+    //
+    // Left subtraction bit by bit, to find the optimal left subtraction solution.
+    //
+    for (SubtractiveMtrrNumber = 0, SubtractiveCount = 1; BaseAddress != 0; SubtractiveCount++) {
+      Alignment = MtrrLibLeastAlignment (BaseAddress, Alignment0);
+
       //
-      // Calculate the alignment of the base address.
+      // Check whether the memory type of [BaseAddress - Alignment, BaseAddress) can override Type.
+      // IA32 Manual defines the following override rules:
+      //   WT > WB
+      //   UC > * (any)
       //
-      Alignment = LShiftU64 (1, (UINTN)LowBitSet64 (BaseAddress));
-
-      if (Alignment > Length) {
+      if (!MtrrLibSubstractable (Ranges, RangeCount, Type, BaseAddress - Alignment, Alignment)) {
         break;
       }
 
-      (*MtrrNumber)++;
-      BaseAddress += Alignment;
-      Length -= Alignment;
-    } while (TRUE);
-
-    if (Length == 0) {
-      return TRUE;
-    }
-  }
-
-  TempQword   = Length;
-  Positive    = 0;
-  Subtractive = 0;
+      for (Index = 0; Index < VariableMtrrCount; Index++) {
+        if ((VariableMtrr[Index].BaseAddress == BaseAddress - Alignment) &&
+            (VariableMtrr[Index].Length == Alignment)) {
+          break;
+        }
+      }
+      if (Index == VariableMtrrCount) {
+        //
+        // Increment SubtractiveMtrrNumber when [BaseAddress - Alignment, BaseAddress) is not be planed as a MTRR
+        //
+        SubtractiveMtrrNumber++;
+      }
 
-  do {
-    TempQword -= Power2MaxMemory (TempQword);
-    Positive++;
-  } while (TempQword != 0);
+      BaseAddress -= Alignment;
+      Length += Alignment;
 
-  TempQword = Power2MaxMemory (LShiftU64 (Length, 1)) - Length;
-  Subtractive++;
-  do {
-    TempQword -= Power2MaxMemory (TempQword);
-    Subtractive++;
-  } while (TempQword != 0);
+      CurrentMtrrNumber = SubtractiveMtrrNumber + MtrrLibGetPositiveMtrrNumber (BaseAddress, Length, Alignment0);
+      if (CurrentMtrrNumber <= LeastLeftMtrrNumber) {
+        LeastLeftMtrrNumber = CurrentMtrrNumber;
+        LeastSubtractiveMtrrNumber = SubtractiveMtrrNumber;
+        *SubLeft = SubtractiveCount;
+        SubtractiveBaseAddress = BaseAddress;
+        SubtractiveLength = Length;
+      }
+    }
 
-  if (Positive <= Subtractive) {
-    *MtrrNumber += Positive;
-    return TRUE;
-  } else {
-    *MtrrNumber += Subtractive;
-    return FALSE;
+    //
+    // If left subtraction is better, subtract BaseAddress to left, and enlarge Length
+    //
+    if (*SubLeft != 0) {
+      BaseAddress = SubtractiveBaseAddress;
+      Length = SubtractiveLength;
+    }
   }
-}
-
-/**
-  Invalid variable MTRRs according to the value in the shadow array.
-
-  This function programs MTRRs according to the values specified
-  in the shadow array.
-
-  @param[in, out]  VariableSettings   Variable MTRR settings
-  @param[in]       VariableMtrrCount  Number of variable MTRRs
-  @param[in, out]  VariableMtrr       Shadow of variable MTRR contents
-
-**/
-VOID
-InvalidateMtrr (
-  IN OUT MTRR_VARIABLE_SETTINGS  *VariableSettings,
-  IN     UINTN                   VariableMtrrCount,
-  IN OUT VARIABLE_MTRR           *VariableMtrr
-  )
-{
-  UINTN         Index;
 
-  for (Index = 0; Index < VariableMtrrCount; Index++) {
-    if (!VariableMtrr[Index].Valid && VariableMtrr[Index].Used) {
-       VariableSettings->Mtrr[Index].Base = 0;
-       VariableSettings->Mtrr[Index].Mask = 0;
-       VariableMtrr[Index].Used = FALSE;
+  //
+  // Increment BaseAddress greedily until (BaseAddress + Alignment) exceeds (BaseAddress + Length)
+  //
+  MiddleMtrrNumber = 0;
+  while (Length != 0) {
+    BaseAlignment = MtrrLibLeastAlignment (BaseAddress, Alignment0);
+    if (BaseAlignment > Length) {
+      break;
     }
+    BaseAddress += BaseAlignment;
+    Length -= BaseAlignment;
+    MiddleMtrrNumber++;
   }
-}
 
 
-/**
-  Programs variable MTRRs
-
-  This function programs variable MTRRs
-
-  @param[in, out]  VariableSettings      Variable MTRR settings.
-  @param[in]       MtrrNumber            Index of MTRR to program.
-  @param[in]       BaseAddress           Base address of memory region.
-  @param[in]       Length                Length of memory region.
-  @param[in]       MemoryCacheType       Memory type to set.
-  @param[in]       MtrrValidAddressMask  The valid address mask for MTRR
+  if (Length == 0) {
+    return LeastSubtractiveMtrrNumber + MiddleMtrrNumber;
+  }
 
-**/
-VOID
-ProgramVariableMtrr (
-  IN OUT MTRR_VARIABLE_SETTINGS  *VariableSettings,
-  IN     UINTN                   MtrrNumber,
-  IN     PHYSICAL_ADDRESS        BaseAddress,
-  IN     UINT64                  Length,
-  IN     UINT64                  MemoryCacheType,
-  IN     UINT64                  MtrrValidAddressMask
-  )
-{
-  UINT64        TempQword;
 
   //
-  // MTRR Physical Base
+  // Get the optimal right subtraction solution.
   //
-  TempQword = (BaseAddress & MtrrValidAddressMask) | MemoryCacheType;
-  VariableSettings->Mtrr[MtrrNumber].Base = TempQword;
 
   //
-  // MTRR Physical Mask
+  // Get the MTRR number needed without right subtraction.
   //
-  TempQword = ~(Length - 1);
-  VariableSettings->Mtrr[MtrrNumber].Mask = (TempQword & MtrrValidAddressMask) | MTRR_LIB_CACHE_MTRR_ENABLED;
+  LeastRightMtrrNumber = MtrrLibGetPositiveMtrrNumber (BaseAddress, Length, Alignment0);
+
+  for (SubtractiveCount = 1; Length < BaseAlignment; SubtractiveCount++) {
+    Alignment = MtrrLibLeastAlignment (BaseAddress + Length, Alignment0);
+    if (!MtrrLibSubstractable (Ranges, RangeCount, Type, BaseAddress + Length, Alignment)) {
+      break;
+    }
+
+    Length += Alignment;
+
+    //
+    // SubtractiveCount = Number of MTRRs used for subtraction
+    //
+    CurrentMtrrNumber = SubtractiveCount + MtrrLibGetPositiveMtrrNumber (BaseAddress, Length, Alignment0);
+    if (CurrentMtrrNumber <= LeastRightMtrrNumber) {
+      LeastRightMtrrNumber = CurrentMtrrNumber;
+      *SubRight = SubtractiveCount;
+      SubtractiveLength = Length;
+    }
+  }
+
+  return LeastSubtractiveMtrrNumber + MiddleMtrrNumber + LeastRightMtrrNumber;
 }
 
 
@@ -1448,33 +1403,461 @@ MtrrDebugPrintAllMtrrs (
   MtrrDebugPrintAllMtrrsWorker (NULL);
 }
 
+/**
+  Update the Ranges array to change the specified range identified by
+  BaseAddress and Length to Type.
+
+  @param Ranges      Array holding memory type settings for all memory regions.
+  @param Capacity    The maximum count of memory ranges the array can hold.
+  @param Count       Return the new memory range count in the array.
+  @param BaseAddress The base address of the memory range to change type.
+  @param Length      The length of the memory range to change type.
+  @param Type        The new type of the specified memory range.
+
+  @retval RETURN_SUCCESS          The type of the specified memory range is
+                                  changed successfully.
+  @retval RETURN_OUT_OF_RESOURCES The new type set causes the count of memory
+                                  range exceeds capacity.
+**/
+RETURN_STATUS
+MtrrLibSetMemoryType (
+  IN MEMORY_RANGE                 *Ranges,
+  IN UINT32                        Capacity,
+  IN OUT UINT32                    *Count,
+  IN UINT64                        BaseAddress,
+  IN UINT64                        Length,
+  IN MTRR_MEMORY_CACHE_TYPE        Type
+  )
+{
+  UINT32                           Index;
+  UINT64                           Limit;
+  UINT64                           LengthLeft;
+  UINT64                           LengthRight;
+  UINT32                           StartIndex;
+  UINT32                           EndIndex;
+  UINT32                           DeltaCount;
+
+  Limit = BaseAddress + Length;
+  StartIndex = *Count;
+  EndIndex = *Count;
+  for (Index = 0; Index < *Count; Index++) {
+    if ((StartIndex == *Count) &&
+        (Ranges[Index].BaseAddress <= BaseAddress) &&
+        (BaseAddress < Ranges[Index].BaseAddress + Ranges[Index].Length)) {
+      StartIndex = Index;
+      LengthLeft = BaseAddress - Ranges[Index].BaseAddress;
+    }
+
+    if ((EndIndex == *Count) &&
+        (Ranges[Index].BaseAddress < Limit) &&
+        (Limit <= Ranges[Index].BaseAddress + Ranges[Index].Length)) {
+      EndIndex = Index;
+      LengthRight = Ranges[Index].BaseAddress + Ranges[Index].Length - Limit;
+      break;
+    }
+  }
+
+  ASSERT (StartIndex != *Count && EndIndex != *Count);
+  if (StartIndex == EndIndex && Ranges[StartIndex].Type == Type) {
+    return RETURN_SUCCESS;
+  }
+
+  //
+  // The type change may cause merging with previous range or next range.
+  // Update the StartIndex, EndIndex, BaseAddress, Length so that following
+  // logic doesn't need to consider merging.
+  //
+  if (StartIndex != 0) {
+    if (LengthLeft == 0 && Ranges[StartIndex - 1].Type == Type) {
+      StartIndex--;
+      Length += Ranges[StartIndex].Length;
+      BaseAddress -= Ranges[StartIndex].Length;
+    }
+  }
+  if (EndIndex != (*Count) - 1) {
+    if (LengthRight == 0 && Ranges[EndIndex + 1].Type == Type) {
+      EndIndex++;
+      Length += Ranges[EndIndex].Length;
+    }
+  }
+
+  //
+  // |- 0 -|- 1 -|- 2 -|- 3 -| StartIndex EndIndex DeltaCount  Count (Count = 4)
+  //   |++++++++++++++++++|    0          3         1=3-0-2    3
+  //   |+++++++|               0          1        -1=1-0-2    5
+  //   |+|                     0          0        -2=0-0-2    6
+  // |+++|                     0          0        -1=0-0-2+1  5
+  //
+  //
+  DeltaCount = EndIndex - StartIndex - 2;
+  if (LengthLeft == 0) {
+    DeltaCount++;
+  }
+  if (LengthRight == 0) {
+    DeltaCount++;
+  }
+  if (*Count - DeltaCount > Capacity) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+
+  //
+  // Reserve (-DeltaCount) space
+  //
+  CopyMem (&Ranges[EndIndex + 1 - DeltaCount], &Ranges[EndIndex + 1], (*Count - EndIndex - 1) * sizeof (Ranges[0]));
+  *Count -= DeltaCount;
+
+  if (LengthLeft != 0) {
+    Ranges[StartIndex].Length = LengthLeft;
+    StartIndex++;
+  }
+  if (LengthRight != 0) {
+    Ranges[EndIndex - DeltaCount].BaseAddress = BaseAddress + Length;
+    Ranges[EndIndex - DeltaCount].Length = LengthRight;
+    Ranges[EndIndex - DeltaCount].Type = Ranges[EndIndex].Type;
+  }
+  Ranges[StartIndex].BaseAddress = BaseAddress;
+  Ranges[StartIndex].Length = Length;
+  Ranges[StartIndex].Type = Type;
+  return RETURN_SUCCESS;
+}
+
+/**
+  Allocate one or more variable MTRR to cover the range identified by
+  BaseAddress and Length.
+
+  @param Ranges               Memory range array holding the memory type
+                              settings for all memory address.
+  @param RangeCount           Count of memory ranges.
+  @param VariableMtrr         Variable MTRR array.
+  @param VariableMtrrCapacity Capacity of variable MTRR array.
+  @param VariableMtrrCount    Count of variable MTRR.
+  @param BaseAddress          Base address of the memory range.
+  @param Length               Length of the memory range.
+  @param Type                 MTRR type of the memory range.
+  @param Alignment0           Alignment of 0.
+
+  @retval RETURN_SUCCESS          Variable MTRRs are allocated successfully.
+  @retval RETURN_OUT_OF_RESOURCES Count of variable MTRRs exceeds capacity.
+**/
+RETURN_STATUS
+MtrrLibSetMemoryAttributeInVariableMtrr (
+  IN CONST MEMORY_RANGE           *Ranges,
+  IN UINT32                        RangeCount,
+  IN OUT VARIABLE_MTRR             *VariableMtrr,
+  IN UINT32                        VariableMtrrCapacity,
+  IN OUT UINT32                    *VariableMtrrCount,
+  IN UINT64                        BaseAddress,
+  IN UINT64                        Length,
+  IN MTRR_MEMORY_CACHE_TYPE        Type,
+  IN UINT64                        Alignment0
+  );
+
+/**
+  Allocate one or more variable MTRR to cover the range identified by
+  BaseAddress and Length.
+
+  The routine recursively calls MtrrLibSetMemoryAttributeInVariableMtrr()
+  to allocate variable MTRRs when the range contains several sub-ranges
+  with different attributes.
+
+  @param Ranges               Memory range array holding the memory type
+                              settings for all memory address.
+  @param RangeCount           Count of memory ranges.
+  @param VariableMtrr         Variable MTRR array.
+  @param VariableMtrrCapacity Capacity of variable MTRR array.
+  @param VariableMtrrCount    Count of variable MTRR.
+  @param BaseAddress          Base address of the memory range.
+  @param Length               Length of the memory range.
+  @param Type                 MTRR type of the range.
+                              If it's CacheInvalid, the memory range may
+                              contains several sub-ranges with different
+                              attributes.
+  @param Alignment0           Alignment of 0.
+
+  @retval RETURN_SUCCESS          Variable MTRRs are allocated successfully.
+  @retval RETURN_OUT_OF_RESOURCES Count of variable MTRRs exceeds capacity.
+**/
+RETURN_STATUS
+MtrrLibAddVariableMtrr (
+  IN CONST MEMORY_RANGE           *Ranges,
+  IN UINT32                        RangeCount,
+  IN OUT VARIABLE_MTRR             *VariableMtrr,
+  IN UINT32                        VariableMtrrCapacity,
+  IN OUT UINT32                    *VariableMtrrCount,
+  IN PHYSICAL_ADDRESS              BaseAddress,
+  IN UINT64                        Length,
+  IN MTRR_MEMORY_CACHE_TYPE        Type,
+  IN UINT64                        Alignment0
+)
+{
+  RETURN_STATUS                    Status;
+  UINT32                           Index;
+  UINT64                           SubLength;
+
+  MTRR_LIB_ASSERT_ALIGNED (BaseAddress, Length);
+  if (Type == CacheInvalid) {
+    for (Index = 0; Index < RangeCount; Index++) {
+      if (Ranges[Index].BaseAddress <= BaseAddress && BaseAddress < Ranges[Index].BaseAddress + Ranges[Index].Length) {
+
+        //
+        // Because the Length may not be aligned to BaseAddress, below code calls
+        // MtrrLibSetMemoryAttributeInVariableMtrr() instead of itself.
+        // MtrrLibSetMemoryAttributeInVariableMtrr() splits the range to several
+        // aligned ranges.
+        //
+        if (Ranges[Index].BaseAddress + Ranges[Index].Length >= BaseAddress + Length) {
+          return MtrrLibSetMemoryAttributeInVariableMtrr (
+            Ranges, RangeCount, VariableMtrr, VariableMtrrCapacity, VariableMtrrCount,
+            BaseAddress, Length, Ranges[Index].Type, Alignment0
+          );
+        } else {
+          SubLength = Ranges[Index].BaseAddress + Ranges[Index].Length - BaseAddress;
+          Status = MtrrLibSetMemoryAttributeInVariableMtrr (
+            Ranges, RangeCount, VariableMtrr, VariableMtrrCapacity, VariableMtrrCount,
+            BaseAddress, SubLength, Ranges[Index].Type, Alignment0
+          );
+          if (RETURN_ERROR (Status)) {
+            return Status;
+          }
+          BaseAddress += SubLength;
+          Length -= SubLength;
+        }
+      }
+    }
+
+    //
+    // Because memory ranges cover all the memory addresses, it's impossible to be here.
+    //
+    ASSERT (FALSE);
+    return RETURN_DEVICE_ERROR;
+  } else {
+    for (Index = 0; Index < *VariableMtrrCount; Index++) {
+      if (VariableMtrr[Index].BaseAddress == BaseAddress && VariableMtrr[Index].Length == Length) {
+        ASSERT (VariableMtrr[Index].Type == Type);
+        break;
+      }
+    }
+    if (Index == *VariableMtrrCount) {
+      if (*VariableMtrrCount == VariableMtrrCapacity) {
+        return RETURN_OUT_OF_RESOURCES;
+      }
+      VariableMtrr[Index].BaseAddress = BaseAddress;
+      VariableMtrr[Index].Length = Length;
+      VariableMtrr[Index].Type = Type;
+      VariableMtrr[Index].Valid = TRUE;
+      VariableMtrr[Index].Used = TRUE;
+      (*VariableMtrrCount)++;
+    }
+    return RETURN_SUCCESS;
+  }
+}
+
+/**
+  Allocate one or more variable MTRR to cover the range identified by
+  BaseAddress and Length.
+
+  @param Ranges               Memory range array holding the memory type
+                              settings for all memory address.
+  @param RangeCount           Count of memory ranges.
+  @param VariableMtrr         Variable MTRR array.
+  @param VariableMtrrCapacity Capacity of variable MTRR array.
+  @param VariableMtrrCount    Count of variable MTRR.
+  @param BaseAddress          Base address of the memory range.
+  @param Length               Length of the memory range.
+  @param Type                 MTRR type of the memory range.
+  @param Alignment0           Alignment of 0.
+
+  @retval RETURN_SUCCESS          Variable MTRRs are allocated successfully.
+  @retval RETURN_OUT_OF_RESOURCES Count of variable MTRRs exceeds capacity.
+**/
+RETURN_STATUS
+MtrrLibSetMemoryAttributeInVariableMtrr (
+  IN CONST MEMORY_RANGE     *Ranges,
+  IN UINT32                 RangeCount,
+  IN OUT VARIABLE_MTRR      *VariableMtrr,
+  IN UINT32                 VariableMtrrCapacity,
+  IN OUT UINT32             *VariableMtrrCount,
+  IN UINT64                 BaseAddress,
+  IN UINT64                 Length,
+  IN MTRR_MEMORY_CACHE_TYPE Type,
+  IN UINT64                 Alignment0
+)
+{
+  UINT64                    Alignment;
+  UINT32                    MtrrNumber;
+  UINT32                    SubtractiveLeft;
+  UINT32                    SubtractiveRight;
+  BOOLEAN                   UseLeastAlignment;
+
+  MtrrNumber = MtrrLibGetMtrrNumber (Ranges, RangeCount, VariableMtrr, *VariableMtrrCount,
+                                     BaseAddress, Length, Type, Alignment0, &SubtractiveLeft, &SubtractiveRight);
+
+  if (MtrrNumber + *VariableMtrrCount > VariableMtrrCapacity) {
+    return RETURN_OUT_OF_RESOURCES;
+  }
+
+  while (SubtractiveLeft-- != 0) {
+    Alignment = MtrrLibLeastAlignment (BaseAddress, Alignment0);
+    ASSERT (Alignment <= Length);
+
+    MtrrLibAddVariableMtrr (Ranges, RangeCount, VariableMtrr, VariableMtrrCapacity, VariableMtrrCount,
+                            BaseAddress - Alignment, Alignment, CacheInvalid, Alignment0);
+    BaseAddress -= Alignment;
+    Length += Alignment;
+  }
+
+  while (Length != 0) {
+    Alignment = MtrrLibLeastAlignment (BaseAddress, Alignment0);
+    if (Alignment > Length) {
+      break;
+    }
+    MtrrLibAddVariableMtrr (NULL, 0, VariableMtrr, VariableMtrrCapacity, VariableMtrrCount,
+                            BaseAddress, Alignment, Type, Alignment0);
+    BaseAddress += Alignment;
+    Length -= Alignment;
+  }
+
+  while (SubtractiveRight-- != 0) {
+    Alignment = MtrrLibLeastAlignment (BaseAddress + Length, Alignment0);
+    MtrrLibAddVariableMtrr (Ranges, RangeCount, VariableMtrr, VariableMtrrCapacity, VariableMtrrCount,
+                            BaseAddress + Length, Alignment, CacheInvalid, Alignment0);
+    Length += Alignment;
+  }
+
+  UseLeastAlignment = TRUE;
+  while (Length != 0) {
+    if (UseLeastAlignment) {
+      Alignment = MtrrLibLeastAlignment (BaseAddress, Alignment0);
+      if (Alignment > Length) {
+        UseLeastAlignment = FALSE;
+      }
+    }
+
+    if (!UseLeastAlignment) {
+      Alignment = GetPowerOfTwo64 (Length);
+    }
+
+    MtrrLibAddVariableMtrr (NULL, 0, VariableMtrr, VariableMtrrCapacity, VariableMtrrCount,
+                            BaseAddress, Alignment, Type, Alignment0);
+    BaseAddress += Alignment;
+    Length -= Alignment;
+  }
+  return RETURN_SUCCESS;
+}
+
+/**
+  Return an array of memory ranges holding memory type settings for all memory
+  address.
+
+  @param DefaultType       The default memory type.
+  @param TotalLength       The total length of the memory.
+  @param VariableMtrr      The variable MTRR array.
+  @param VariableMtrrCount The count of variable MTRRs.
+  @param Ranges            Return the memory range array holding memory type
+                           settings for all memory address.
+  @param RangeCapacity     The capacity of memory range array.
+  @param RangeCount        Return the count of memory range.
+
+  @retval RETURN_SUCCESS          The memory range array is returned successfully.
+  @retval RETURN_OUT_OF_RESOURCES The count of memory ranges exceeds capacity.
+**/
+RETURN_STATUS
+MtrrLibGetMemoryTypes (
+  IN MTRR_MEMORY_CACHE_TYPE      DefaultType,
+  IN UINT64                      TotalLength,
+  IN CONST VARIABLE_MTRR         *VariableMtrr,
+  IN UINT32                      VariableMtrrCount,
+  OUT MEMORY_RANGE               *Ranges,
+  IN UINT32                      RangeCapacity,
+  OUT UINT32                     *RangeCount
+)
+{
+  RETURN_STATUS                  Status;
+  UINTN                          Index;
+
+  //
+  // WT > WB
+  // UC > *
+  // UC > * (except WB, UC) > WB
+  //
+
+  //
+  // 0. Set whole range as DefaultType
+  //
+  *RangeCount = 1;
+  Ranges[0].BaseAddress = 0;
+  Ranges[0].Length = TotalLength;
+  Ranges[0].Type = DefaultType;
+
+  //
+  // 1. Set WB
+  //
+  for (Index = 0; Index < VariableMtrrCount; Index++) {
+    if (VariableMtrr[Index].Valid && VariableMtrr[Index].Type == CacheWriteBack) {
+      Status = MtrrLibSetMemoryType (
+        Ranges, RangeCapacity, RangeCount,
+        VariableMtrr[Index].BaseAddress, VariableMtrr[Index].Length, (MTRR_MEMORY_CACHE_TYPE) VariableMtrr[Index].Type
+      );
+      if (RETURN_ERROR (Status)) {
+        return Status;
+      }
+    }
+  }
+
+  //
+  // 2. Set other types than WB or UC
+  //
+  for (Index = 0; Index < VariableMtrrCount; Index++) {
+    if (VariableMtrr[Index].Valid && VariableMtrr[Index].Type != CacheWriteBack && VariableMtrr[Index].Type != CacheUncacheable) {
+      Status = MtrrLibSetMemoryType (
+        Ranges, RangeCapacity, RangeCount,
+        VariableMtrr[Index].BaseAddress, VariableMtrr[Index].Length, (MTRR_MEMORY_CACHE_TYPE) VariableMtrr[Index].Type
+      );
+      if (RETURN_ERROR (Status)) {
+        return Status;
+      }
+    }
+  }
+
+  //
+  // 3. Set UC
+  //
+  for (Index = 0; Index < VariableMtrrCount; Index++) {
+    if (VariableMtrr[Index].Valid && VariableMtrr[Index].Type == CacheUncacheable) {
+      Status = MtrrLibSetMemoryType (
+        Ranges, RangeCapacity, RangeCount,
+        VariableMtrr[Index].BaseAddress, VariableMtrr[Index].Length, (MTRR_MEMORY_CACHE_TYPE) VariableMtrr[Index].Type
+      );
+      if (RETURN_ERROR (Status)) {
+        return Status;
+      }
+    }
+  }
+  return RETURN_SUCCESS;
+}
 
 /**
   Worker function attempts to set the attributes for a memory range.
 
-  If MtrrSettings is not NULL, set the attributes into the input MTRR
+  If MtrrSetting is not NULL, set the attributes into the input MTRR
   settings buffer.
-  If MtrrSettings is NULL, set the attributes into MTRRs registers.
+  If MtrrSetting is NULL, set the attributes into MTRRs registers.
 
   @param[in, out]  MtrrSetting       A buffer holding all MTRRs content.
   @param[in]       BaseAddress       The physical address that is the start
-                                     address of a memory region.
-  @param[in]       Length            The size in bytes of the memory region.
-  @param[in]       Attribute         The bit mask of attributes to set for the
-                                     memory region.
+                                     address of a memory range.
+  @param[in]       Length            The size in bytes of the memory range.
+  @param[in]       Type              The MTRR type to set for the memory range.
 
   @retval RETURN_SUCCESS            The attributes were set for the memory
-                                    region.
+                                    range.
   @retval RETURN_INVALID_PARAMETER  Length is zero.
   @retval RETURN_UNSUPPORTED        The processor does not support one or
                                     more bytes of the memory resource range
                                     specified by BaseAddress and Length.
-  @retval RETURN_UNSUPPORTED        The bit mask of attributes is not support
-                                    for the memory resource range specified
+  @retval RETURN_UNSUPPORTED        The MTRR type is not support for the
+                                    memory resource range specified
                                     by BaseAddress and Length.
-  @retval RETURN_ACCESS_DENIED      The attributes for the memory resource
-                                    range specified by BaseAddress and Length
-                                    cannot be modified.
   @retval RETURN_OUT_OF_RESOURCES   There are not enough system resources to
                                     modify the attributes of the memory
                                     resource range.
@@ -1485,97 +1868,80 @@ MtrrSetMemoryAttributeWorker (
   IN OUT MTRR_SETTINGS           *MtrrSetting,
   IN PHYSICAL_ADDRESS            BaseAddress,
   IN UINT64                      Length,
-  IN MTRR_MEMORY_CACHE_TYPE      Attribute
+  IN MTRR_MEMORY_CACHE_TYPE      Type
   )
 {
-  UINT64                    TempQword;
   RETURN_STATUS             Status;
-  UINT64                    MemoryType;
-  UINT64                    Alignment;
-  BOOLEAN                   OverLap;
-  BOOLEAN                   Positive;
-  UINT32                    MsrNum;
-  UINTN                     MtrrNumber;
-  VARIABLE_MTRR             VariableMtrr[MTRR_NUMBER_OF_VARIABLE_MTRR];
-  UINT32                    UsedMtrr;
+  UINT32                    Index;
+  UINT32                    WorkingIndex;
+  //
+  // N variable MTRRs can maximumly separate (2N + 1) Ranges, plus 1 range for [0, 1M).
+  //
+  MEMORY_RANGE              Ranges[MTRR_NUMBER_OF_VARIABLE_MTRR * 2 + 2];
+  UINT32                    RangeCount;
   UINT64                    MtrrValidBitsMask;
   UINT64                    MtrrValidAddressMask;
-  BOOLEAN                   OverwriteExistingMtrr;
-  UINT32                    FirmwareVariableMtrrCount;
+  UINT64                    Alignment0;
   MTRR_CONTEXT              MtrrContext;
   BOOLEAN                   MtrrContextValid;
-  BOOLEAN                   FixedSettingsValid[MTRR_NUMBER_OF_FIXED_MTRR];
-  BOOLEAN                   FixedSettingsModified[MTRR_NUMBER_OF_FIXED_MTRR];
-  MTRR_FIXED_SETTINGS       WorkingFixedSettings;
-  UINT32                    VariableMtrrCount;
-  MTRR_VARIABLE_SETTINGS    OriginalVariableSettings;
-  BOOLEAN                   ProgramVariableSettings;
-  MTRR_VARIABLE_SETTINGS    WorkingVariableSettings;
-  UINT32                    Index;
+
+  MTRR_MEMORY_CACHE_TYPE    DefaultType;
+
+  UINT32                    MsrIndex;
   UINT64                    ClearMask;
   UINT64                    OrMask;
   UINT64                    NewValue;
-  MTRR_VARIABLE_SETTINGS    *VariableSettings;
+  BOOLEAN                   FixedSettingsValid[MTRR_NUMBER_OF_FIXED_MTRR];
+  BOOLEAN                   FixedSettingsModified[MTRR_NUMBER_OF_FIXED_MTRR];
+  MTRR_FIXED_SETTINGS       WorkingFixedSettings;
 
-  MtrrContextValid  = FALSE;
-  VariableMtrrCount = 0;
-  ZeroMem (&WorkingFixedSettings, sizeof (WorkingFixedSettings));
-  for (Index = 0; Index < MTRR_NUMBER_OF_FIXED_MTRR; Index++) {
-    FixedSettingsValid[Index]    = FALSE;
-    FixedSettingsModified[Index] = FALSE;
-  }
-  ProgramVariableSettings = FALSE;
+  UINT32                    FirmwareVariableMtrrCount;
+  MTRR_VARIABLE_SETTINGS    *VariableSettings;
+  MTRR_VARIABLE_SETTINGS    OriginalVariableSettings;
+  UINT32                    OriginalVariableMtrrCount;
+  VARIABLE_MTRR             OriginalVariableMtrr[MTRR_NUMBER_OF_VARIABLE_MTRR];
+  UINT32                    WorkingVariableMtrrCount;
+  VARIABLE_MTRR             WorkingVariableMtrr[MTRR_NUMBER_OF_VARIABLE_MTRR];
+  BOOLEAN                   VariableSettingModified[MTRR_NUMBER_OF_VARIABLE_MTRR];
+  UINTN                     FreeVariableMtrrCount;
 
-  if (!IsMtrrSupported ()) {
-    Status = RETURN_UNSUPPORTED;
-    goto Done;
+  if (Length == 0) {
+    return RETURN_INVALID_PARAMETER;
   }
 
   MtrrLibInitializeMtrrMask (&MtrrValidBitsMask, &MtrrValidAddressMask);
-
-  TempQword = 0;
-  MemoryType = (UINT64)Attribute;
-  OverwriteExistingMtrr = FALSE;
-
-  //
-  // Check for an invalid parameter
-  //
-  if (Length == 0) {
-    Status = RETURN_INVALID_PARAMETER;
-    goto Done;
+  if (((BaseAddress & ~MtrrValidAddressMask) != 0) || (Length & ~MtrrValidAddressMask) != 0) {
+    return RETURN_UNSUPPORTED;
   }
 
-  if (
-       (BaseAddress & ~MtrrValidAddressMask) != 0 ||
-       (Length & ~MtrrValidAddressMask) != 0
-     ) {
-    Status = RETURN_UNSUPPORTED;
-    goto Done;
+  ZeroMem (&WorkingFixedSettings, sizeof (WorkingFixedSettings));
+  for (Index = 0; Index < MTRR_NUMBER_OF_FIXED_MTRR; Index++) {
+    FixedSettingsValid[Index]    = FALSE;
+    FixedSettingsModified[Index] = FALSE;
   }
 
   //
   // Check if Fixed MTRR
   //
-  Status = RETURN_SUCCESS;
   if (BaseAddress < BASE_1MB) {
-    MsrNum = (UINT32)-1;
-    while ((BaseAddress < BASE_1MB) && (Length > 0) && Status == RETURN_SUCCESS) {
-      Status = MtrrLibProgramFixedMtrr (Attribute, &BaseAddress, &Length, &MsrNum, &ClearMask, &OrMask);
+    MsrIndex = (UINT32)-1;
+    while ((BaseAddress < BASE_1MB) && (Length != 0)) {
+      Status = MtrrLibProgramFixedMtrr (Type, &BaseAddress, &Length, &MsrIndex, &ClearMask, &OrMask);
       if (RETURN_ERROR (Status)) {
-        goto Done;
+        return Status;
       }
       if (MtrrSetting != NULL) {
-        MtrrSetting->Fixed.Mtrr[MsrNum] = (MtrrSetting->Fixed.Mtrr[MsrNum] & ~ClearMask) | OrMask;
-        MtrrSetting->MtrrDefType |= MTRR_LIB_CACHE_FIXED_MTRR_ENABLED;
+        MtrrSetting->Fixed.Mtrr[MsrIndex] = (MtrrSetting->Fixed.Mtrr[MsrIndex] & ~ClearMask) | OrMask;
+        ((MSR_IA32_MTRR_DEF_TYPE_REGISTER *) &MtrrSetting->MtrrDefType)->Bits.FE = 1;
       } else {
-        if (!FixedSettingsValid[MsrNum]) {
-          WorkingFixedSettings.Mtrr[MsrNum] = AsmReadMsr64 (mMtrrLibFixedMtrrTable[MsrNum].Msr);
-          FixedSettingsValid[MsrNum] = TRUE;
+        if (!FixedSettingsValid[MsrIndex]) {
+          WorkingFixedSettings.Mtrr[MsrIndex] = AsmReadMsr64 (mMtrrLibFixedMtrrTable[MsrIndex].Msr);
+          FixedSettingsValid[MsrIndex] = TRUE;
         }
-        NewValue = (WorkingFixedSettings.Mtrr[MsrNum] & ~ClearMask) | OrMask;
-        if (WorkingFixedSettings.Mtrr[MsrNum] != NewValue) {
-          WorkingFixedSettings.Mtrr[MsrNum] = NewValue;
-          FixedSettingsModified[MsrNum] = TRUE;
+        NewValue = (WorkingFixedSettings.Mtrr[MsrIndex] & ~ClearMask) | OrMask;
+        if (WorkingFixedSettings.Mtrr[MsrIndex] != NewValue) {
+          WorkingFixedSettings.Mtrr[MsrIndex] = NewValue;
+          FixedSettingsModified[MsrIndex] = TRUE;
         }
       }
     }
@@ -1591,198 +1957,178 @@ MtrrSetMemoryAttributeWorker (
   }
 
   //
-  // Since memory ranges below 1MB will be overridden by the fixed MTRRs,
-  // we can set the base to 0 to save variable MTRRs.
+  // Read the default MTRR type
   //
-  if (BaseAddress == BASE_1MB) {
-    BaseAddress = 0;
-    Length += SIZE_1MB;
-  }
+  DefaultType = MtrrGetDefaultMemoryTypeWorker (MtrrSetting);
 
   //
-  // Read all variable MTRRs
+  // Read all variable MTRRs and convert to Ranges.
   //
-  VariableMtrrCount = GetVariableMtrrCountWorker ();
-  FirmwareVariableMtrrCount = GetFirmwareVariableMtrrCountWorker ();
-  if (MtrrSetting != NULL) {
-    VariableSettings = &MtrrSetting->Variables;
+  OriginalVariableMtrrCount = GetVariableMtrrCountWorker ();
+  if (MtrrSetting == NULL) {
+    ZeroMem (&OriginalVariableSettings, sizeof (OriginalVariableSettings));
+    MtrrGetVariableMtrrWorker (NULL, OriginalVariableMtrrCount, &OriginalVariableSettings);
+    VariableSettings = &OriginalVariableSettings;
   } else {
-    MtrrGetVariableMtrrWorker (NULL, VariableMtrrCount, &OriginalVariableSettings);
-    CopyMem (&WorkingVariableSettings, &OriginalVariableSettings, sizeof (WorkingVariableSettings));
-    ProgramVariableSettings = TRUE;
-    VariableSettings = &WorkingVariableSettings;
-  }
-
-  //
-  // Check for overlap
-  //
-  UsedMtrr = MtrrGetMemoryAttributeInVariableMtrrWorker (
-               VariableSettings,
-               FirmwareVariableMtrrCount,
-               MtrrValidBitsMask,
-               MtrrValidAddressMask,
-               VariableMtrr
-               );
-  OverLap = CheckMemoryAttributeOverlap (
-              FirmwareVariableMtrrCount,
-              BaseAddress,
-              BaseAddress + Length - 1,
-              VariableMtrr
-              );
-  if (OverLap) {
-    Status = CombineMemoryAttribute (
-               FirmwareVariableMtrrCount,
-               MemoryType,
-               &BaseAddress,
-               &Length,
-               VariableMtrr,
-               &UsedMtrr,
-               &OverwriteExistingMtrr
-               );
-    if (RETURN_ERROR (Status)) {
-      goto Done;
-    }
-
-    if (Length == 0) {
-      //
-      // Combined successfully, invalidate the now-unused MTRRs
-      //
-      InvalidateMtrr (VariableSettings, VariableMtrrCount, VariableMtrr);
-      Status = RETURN_SUCCESS;
-      goto Done;
-    }
+    VariableSettings = &MtrrSetting->Variables;
   }
+  MtrrGetMemoryAttributeInVariableMtrrWorker (VariableSettings, OriginalVariableMtrrCount, MtrrValidBitsMask, MtrrValidAddressMask, OriginalVariableMtrr);
+
+  Status = MtrrLibGetMemoryTypes (
+    DefaultType, MtrrValidBitsMask + 1, OriginalVariableMtrr, OriginalVariableMtrrCount,
+    Ranges, 2 * OriginalVariableMtrrCount + 1, &RangeCount
+  );
+  ASSERT (Status == RETURN_SUCCESS);
+
+  FirmwareVariableMtrrCount = GetFirmwareVariableMtrrCountWorker ();
+  ASSERT (RangeCount <= 2 * FirmwareVariableMtrrCount + 1);
 
   //
-  // The memory type is the same with the type specified by
-  // MTRR_LIB_IA32_MTRR_DEF_TYPE.
+  // Force [0, 1M) to UC, so that it doesn't impact left subtraction algorithm.
   //
-  if ((!OverwriteExistingMtrr) && (Attribute == MtrrGetDefaultMemoryTypeWorker (MtrrSetting))) {
-    //
-    // Invalidate the now-unused MTRRs
-    //
-    InvalidateMtrr (VariableSettings, VariableMtrrCount, VariableMtrr);
-    goto Done;
+  Status = MtrrLibSetMemoryType (Ranges, 2 * FirmwareVariableMtrrCount + 2, &RangeCount, 0, SIZE_1MB, CacheUncacheable);
+  ASSERT (Status == RETURN_SUCCESS);
+  //
+  // Apply Type to [BaseAddress, BaseAddress + Length)
+  //
+  Status = MtrrLibSetMemoryType (Ranges, 2 * FirmwareVariableMtrrCount + 2, &RangeCount, BaseAddress, Length, Type);
+  if (RETURN_ERROR (Status)) {
+    return Status;
   }
 
-  Positive = GetMtrrNumberAndDirection (BaseAddress, Length, &MtrrNumber);
-
-  if ((UsedMtrr + MtrrNumber) > FirmwareVariableMtrrCount) {
-    Status = RETURN_OUT_OF_RESOURCES;
-    goto Done;
+  Alignment0 = LShiftU64 (1, (UINTN) HighBitSet64 (MtrrValidBitsMask));
+  WorkingVariableMtrrCount = 0;
+  ZeroMem (&WorkingVariableMtrr, sizeof (WorkingVariableMtrr));
+  for (Index = 0; Index < RangeCount; Index++) {
+    if (Ranges[Index].Type != DefaultType) {
+      //
+      // Maximum allowed MTRR count is (FirmwareVariableMtrrCount + 1)
+      // Because potentially the range [0, 1MB) is not merged, but can be ignored because fixed MTRR covers that
+      //
+      Status = MtrrLibSetMemoryAttributeInVariableMtrr (
+        Ranges, RangeCount,
+        WorkingVariableMtrr, FirmwareVariableMtrrCount + 1, &WorkingVariableMtrrCount,
+        Ranges[Index].BaseAddress, Ranges[Index].Length,
+        Ranges[Index].Type, Alignment0
+      );
+      if (RETURN_ERROR (Status)) {
+        return Status;
+      }
+    }
   }
 
   //
-  // Invalidate the now-unused MTRRs
+  // Remove the [0, 1MB) MTRR if it still exists (not merged with other range)
   //
-  InvalidateMtrr (VariableSettings, VariableMtrrCount, VariableMtrr);
+  if (WorkingVariableMtrr[0].BaseAddress == 0 && WorkingVariableMtrr[0].Length == SIZE_1MB) {
+    ASSERT (WorkingVariableMtrr[0].Type == CacheUncacheable);
+    WorkingVariableMtrrCount--;
+    CopyMem (&WorkingVariableMtrr[0], &WorkingVariableMtrr[1], WorkingVariableMtrrCount * sizeof (VARIABLE_MTRR));
+  }
 
-  //
-  // Find first unused MTRR
-  //
-  for (MsrNum = 0; MsrNum < VariableMtrrCount; MsrNum++) {
-    if ((VariableSettings->Mtrr[MsrNum].Mask & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
-      break;
-    }
+  if (WorkingVariableMtrrCount > FirmwareVariableMtrrCount) {
+    return RETURN_OUT_OF_RESOURCES;
   }
 
-  if (BaseAddress != 0) {
-    do {
-      //
-      // Calculate the alignment of the base address.
-      //
-      Alignment = LShiftU64 (1, (UINTN)LowBitSet64 (BaseAddress));
+  for (Index = 0; Index < OriginalVariableMtrrCount; Index++) {
+    VariableSettingModified[Index] = FALSE;
 
-      if (Alignment > Length) {
+    if (!OriginalVariableMtrr[Index].Valid) {
+      continue;
+    }
+    for (WorkingIndex = 0; WorkingIndex < WorkingVariableMtrrCount; WorkingIndex++) {
+      if (OriginalVariableMtrr[Index].BaseAddress == WorkingVariableMtrr[WorkingIndex].BaseAddress &&
+          OriginalVariableMtrr[Index].Length == WorkingVariableMtrr[WorkingIndex].Length &&
+          OriginalVariableMtrr[Index].Type == WorkingVariableMtrr[WorkingIndex].Type) {
         break;
       }
+    }
 
+    if (WorkingIndex == WorkingVariableMtrrCount) {
       //
-      // Find unused MTRR
+      // Remove the one from OriginalVariableMtrr which is not in WorkingVariableMtrr
       //
-      for (; MsrNum < VariableMtrrCount; MsrNum++) {
-        if ((VariableSettings->Mtrr[MsrNum].Mask & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
+      OriginalVariableMtrr[Index].Valid = FALSE;
+      VariableSettingModified[Index] = TRUE;
+    } else {
+      //
+      // Remove the one from WorkingVariableMtrr which is also in OriginalVariableMtrr
+      //
+      WorkingVariableMtrr[WorkingIndex].Valid = FALSE;
+    }
+    //
+    // The above two operations cause that valid MTRR only exists in either OriginalVariableMtrr or WorkingVariableMtrr.
+    //
+  }
+
+  //
+  // Merge remaining MTRRs from WorkingVariableMtrr to OriginalVariableMtrr
+  //
+  for (FreeVariableMtrrCount = 0, WorkingIndex = 0, Index = 0; Index < OriginalVariableMtrrCount; Index++) {
+    if (!OriginalVariableMtrr[Index].Valid) {
+      for (; WorkingIndex < WorkingVariableMtrrCount; WorkingIndex++) {
+        if (WorkingVariableMtrr[WorkingIndex].Valid) {
           break;
         }
       }
-
-      ProgramVariableMtrr (
-        VariableSettings,
-        MsrNum,
-        BaseAddress,
-        Alignment,
-        MemoryType,
-        MtrrValidAddressMask
-        );
-      BaseAddress += Alignment;
-      Length -= Alignment;
-    } while (TRUE);
-
-    if (Length == 0) {
-      goto Done;
+      if (WorkingIndex == WorkingVariableMtrrCount) {
+        FreeVariableMtrrCount++;
+      } else {
+        CopyMem (&OriginalVariableMtrr[Index], &WorkingVariableMtrr[WorkingIndex], sizeof (VARIABLE_MTRR));
+        VariableSettingModified[Index] = TRUE;
+        WorkingIndex++;
+      }
     }
   }
+  ASSERT (OriginalVariableMtrrCount - FreeVariableMtrrCount <= FirmwareVariableMtrrCount);
 
-  TempQword = Length;
-
-  if (!Positive) {
-    Length = Power2MaxMemory (LShiftU64 (TempQword, 1));
+  //
+  // Move MTRRs after the FirmwraeVariableMtrrCount position to beginning
+  //
+  WorkingIndex = FirmwareVariableMtrrCount;
+  for (Index = 0; Index < FirmwareVariableMtrrCount; Index++) {
+    if (!OriginalVariableMtrr[Index].Valid) {
+      //
+      // Found an empty MTRR in WorkingIndex position
+      //
+      for (; WorkingIndex < OriginalVariableMtrrCount; WorkingIndex++) {
+        if (OriginalVariableMtrr[WorkingIndex].Valid) {
+          break;
+        }
+      }
 
-    //
-    // Find unused MTRR
-    //
-    for (; MsrNum < VariableMtrrCount; MsrNum++) {
-      if ((VariableSettings->Mtrr[MsrNum].Mask & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
-        break;
+      if (WorkingIndex != OriginalVariableMtrrCount) {
+        CopyMem (&OriginalVariableMtrr[Index], &OriginalVariableMtrr[WorkingIndex], sizeof (VARIABLE_MTRR));
+        VariableSettingModified[Index] = TRUE;
+        VariableSettingModified[WorkingIndex] = TRUE;
+        OriginalVariableMtrr[WorkingIndex].Valid = FALSE;
       }
     }
-
-    ProgramVariableMtrr (
-      VariableSettings,
-      MsrNum,
-      BaseAddress,
-      Length,
-      MemoryType,
-      MtrrValidAddressMask
-      );
-    BaseAddress += Length;
-    TempQword   = Length - TempQword;
-    MemoryType  = MTRR_CACHE_UNCACHEABLE;
   }
 
-  do {
-    //
-    // Find unused MTRR
-    //
-    for (; MsrNum < VariableMtrrCount; MsrNum++) {
-      if ((VariableSettings->Mtrr[MsrNum].Mask & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
-        break;
+  //
+  // Convert OriginalVariableMtrr to VariableSettings
+  // NOTE: MTRR from FirmwareVariableMtrr to OriginalVariableMtrr need to update as well.
+  //
+  for (Index = 0; Index < OriginalVariableMtrrCount; Index++) {
+    if (VariableSettingModified[Index]) {
+      if (OriginalVariableMtrr[Index].Valid) {
+        VariableSettings->Mtrr[Index].Base = (OriginalVariableMtrr[Index].BaseAddress & MtrrValidAddressMask) | (UINT8) OriginalVariableMtrr[Index].Type;
+        VariableSettings->Mtrr[Index].Mask = (~(OriginalVariableMtrr[Index].Length - 1)) & MtrrValidAddressMask | BIT11;
+      } else {
+        VariableSettings->Mtrr[Index].Base = 0;
+        VariableSettings->Mtrr[Index].Mask = 0;
       }
     }
-
-    Length = Power2MaxMemory (TempQword);
-    if (!Positive) {
-      BaseAddress -= Length;
-    }
-
-    ProgramVariableMtrr (
-      VariableSettings,
-      MsrNum,
-      BaseAddress,
-      Length,
-      MemoryType,
-      MtrrValidAddressMask
-      );
-
-    if (Positive) {
-      BaseAddress += Length;
-    }
-    TempQword -= Length;
-
-  } while (TempQword > 0);
+  }
 
 Done:
+  if (MtrrSetting != NULL) {
+    ((MSR_IA32_MTRR_DEF_TYPE_REGISTER *) &MtrrSetting->MtrrDefType)->Bits.E = 1;
+    return RETURN_SUCCESS;
+  }
 
+  MtrrContextValid = FALSE;
   //
   // Write fixed MTRRs that have been modified
   //
@@ -1802,37 +2148,26 @@ Done:
   //
   // Write variable MTRRs
   //
-  if (ProgramVariableSettings) {
-    for (Index = 0; Index < VariableMtrrCount; Index++) {
-      if (WorkingVariableSettings.Mtrr[Index].Base != OriginalVariableSettings.Mtrr[Index].Base ||
-          WorkingVariableSettings.Mtrr[Index].Mask != OriginalVariableSettings.Mtrr[Index].Mask    ) {
-        if (!MtrrContextValid) {
-          MtrrLibPreMtrrChange (&MtrrContext);
-          MtrrContextValid = TRUE;
-        }
-        AsmWriteMsr64 (
-          MTRR_LIB_IA32_VARIABLE_MTRR_BASE + (Index << 1),
-          WorkingVariableSettings.Mtrr[Index].Base
-          );
-        AsmWriteMsr64 (
-          MTRR_LIB_IA32_VARIABLE_MTRR_BASE + (Index << 1) + 1,
-          WorkingVariableSettings.Mtrr[Index].Mask
-          );
+  for (Index = 0; Index < OriginalVariableMtrrCount; Index++) {
+    if (VariableSettingModified[Index]) {
+      if (!MtrrContextValid) {
+        MtrrLibPreMtrrChange (&MtrrContext);
+        MtrrContextValid = TRUE;
       }
+      AsmWriteMsr64 (
+        MSR_IA32_MTRR_PHYSBASE0 + (Index << 1),
+        VariableSettings->Mtrr[Index].Base
+      );
+      AsmWriteMsr64 (
+        MSR_IA32_MTRR_PHYSMASK0 + (Index << 1),
+        VariableSettings->Mtrr[Index].Mask
+      );
     }
   }
   if (MtrrContextValid) {
     MtrrLibPostMtrrChange (&MtrrContext);
   }
 
-  DEBUG((DEBUG_CACHE, "  Status = %r\n", Status));
-  if (!RETURN_ERROR (Status)) {
-    if (MtrrSetting != NULL) {
-      MtrrSetting->MtrrDefType |= MTRR_LIB_CACHE_MTRR_ENABLED;
-    }
-    MtrrDebugPrintAllMtrrsWorker (MtrrSetting);
-  }
-
   return Status;
 }
 
@@ -1840,13 +2175,13 @@ Done:
   This function attempts to set the attributes for a memory range.
 
   @param[in]  BaseAddress        The physical address that is the start
-                                 address of a memory region.
-  @param[in]  Length             The size in bytes of the memory region.
+                                 address of a memory range.
+  @param[in]  Length             The size in bytes of the memory range.
   @param[in]  Attributes         The bit mask of attributes to set for the
-                                 memory region.
+                                 memory range.
 
   @retval RETURN_SUCCESS            The attributes were set for the memory
-                                    region.
+                                    range.
   @retval RETURN_INVALID_PARAMETER  Length is zero.
   @retval RETURN_UNSUPPORTED        The processor does not support one or
                                     more bytes of the memory resource range
@@ -1870,13 +2205,20 @@ MtrrSetMemoryAttribute (
   IN MTRR_MEMORY_CACHE_TYPE  Attribute
   )
 {
-  DEBUG((DEBUG_CACHE, "MtrrSetMemoryAttribute() %a:%016lx-%016lx\n", mMtrrMemoryCacheTypeShortName[Attribute], BaseAddress, Length));
-  return MtrrSetMemoryAttributeWorker (
-           NULL,
-           BaseAddress,
-           Length,
-           Attribute
-           );
+  RETURN_STATUS              Status;
+
+  if (!IsMtrrSupported ()) {
+    return RETURN_UNSUPPORTED;
+  }
+
+  Status = MtrrSetMemoryAttributeWorker (NULL, BaseAddress, Length, Attribute);
+  DEBUG ((DEBUG_CACHE, "MtrrSetMemoryAttribute() %a: [%016lx, %016lx) - %r\n",
+          mMtrrMemoryCacheTypeShortName[Attribute], BaseAddress, BaseAddress + Length, Status));
+
+  if (!RETURN_ERROR (Status)) {
+    MtrrDebugPrintAllMtrrsWorker (NULL);
+  }
+  return Status;
 }
 
 /**
@@ -1884,12 +2226,12 @@ MtrrSetMemoryAttribute (
 
   @param[in, out]  MtrrSetting  MTRR setting buffer to be set.
   @param[in]       BaseAddress  The physical address that is the start address
-                                of a memory region.
-  @param[in]       Length       The size in bytes of the memory region.
+                                of a memory range.
+  @param[in]       Length       The size in bytes of the memory range.
   @param[in]       Attribute    The bit mask of attributes to set for the
-                                memory region.
+                                memory range.
 
-  @retval RETURN_SUCCESS            The attributes were set for the memory region.
+  @retval RETURN_SUCCESS            The attributes were set for the memory range.
   @retval RETURN_INVALID_PARAMETER  Length is zero.
   @retval RETURN_UNSUPPORTED        The processor does not support one or more bytes of the
                                     memory resource range specified by BaseAddress and Length.
@@ -1910,13 +2252,16 @@ MtrrSetMemoryAttributeInMtrrSettings (
   IN MTRR_MEMORY_CACHE_TYPE  Attribute
   )
 {
-  DEBUG((DEBUG_CACHE, "MtrrSetMemoryAttributeMtrrSettings(%p) %a:%016lx-%016lx\n", MtrrSetting, mMtrrMemoryCacheTypeShortName[Attribute], BaseAddress, Length));
-  return MtrrSetMemoryAttributeWorker (
-           MtrrSetting,
-           BaseAddress,
-           Length,
-           Attribute
-           );
+  RETURN_STATUS              Status;
+  Status = MtrrSetMemoryAttributeWorker (MtrrSetting, BaseAddress, Length, Attribute);
+  DEBUG((DEBUG_CACHE, "MtrrSetMemoryAttributeMtrrSettings(%p) %a: [%016lx, %016lx) - %r\n",
+         MtrrSetting, mMtrrMemoryCacheTypeShortName[Attribute], BaseAddress, BaseAddress + Length, Status));
+
+  if (!RETURN_ERROR (Status)) {
+    MtrrDebugPrintAllMtrrsWorker (MtrrSetting);
+  }
+
+  return Status;
 }
 
 /**
@@ -2147,3 +2492,4 @@ IsMtrrSupported (
   }
   return TRUE;
 }
+
-- 
2.9.0.windows.1