From 1a2ad6fca7414241807bd0b3143a914bd46deb75 Mon Sep 17 00:00:00 2001
From: rsun3 <rsun3@6f19259b-4bc3-4df7-8a09-765794883524>
Date: Tue, 13 Sep 2011 03:06:48 +0000
Subject: [PATCH] Enhance the MTRR lib to support the case where alignment of
 base address < length.

Signed-off by: rsun3
Reviewed-by:   hhuan13

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@12330 6f19259b-4bc3-4df7-8a09-765794883524
---
 UefiCpuPkg/Library/MtrrLib/MtrrLib.c | 217 ++++++++++++++++-----------
 1 file changed, 126 insertions(+), 91 deletions(-)

diff --git a/UefiCpuPkg/Library/MtrrLib/MtrrLib.c b/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
index cc4f5b37d0..edb75c04a5 100644
--- a/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
+++ b/UefiCpuPkg/Library/MtrrLib/MtrrLib.c
@@ -586,12 +586,17 @@ Power2MaxMemory (
 
 
 /**
-  Check the direction to program variable MTRRs.
+  Determine the MTRR numbers used to program a memory range.
 
-  This function determines which direction of programming the variable
-  MTRRs will use fewer MTRRs.
+  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.
 
-  @param  Input       Length of Memory to program MTRR
+  Then this function determines which direction of programming the variable MTRRs for the remaining length
+  will use fewer MTRRs.
+
+  @param  BaseAddress Length of Memory to program MTRR
+  @param  Length      Length of Memory to program MTRR
   @param  MtrrNumber  Pointer to the number of necessary MTRRs
 
   @retval TRUE        Positive direction is better.
@@ -599,16 +604,41 @@ Power2MaxMemory (
 
 **/
 BOOLEAN
-GetDirection (
-  IN UINT64      Input,
+GetMtrrNumberAndDirection (
+  IN UINT64      BaseAddress,
+  IN UINT64      Length,
   IN UINTN       *MtrrNumber
   )
 {
   UINT64  TempQword;
+  UINT64  Alignment;
   UINT32  Positive;
   UINT32  Subtractive;
 
-  TempQword   = Input;
+  *MtrrNumber = 0;
+
+  if (BaseAddress != 0) {
+    do {
+      //
+      // Calculate the alignment of the base address.
+      //
+      Alignment = LShiftU64 (1, (UINTN)LowBitSet64 (BaseAddress));
+
+      if (Alignment > Length) {
+        break;
+      }
+
+      (*MtrrNumber)++;
+      BaseAddress += Alignment;
+      Length -= Alignment;
+    } while (TRUE);
+
+    if (Length == 0) {
+      return TRUE;
+    }
+  }
+
+  TempQword   = Length;
   Positive    = 0;
   Subtractive = 0;
 
@@ -617,7 +647,7 @@ GetDirection (
     Positive++;
   } while (TempQword != 0);
 
-  TempQword = Power2MaxMemory (LShiftU64 (Input, 1)) - Input;
+  TempQword = Power2MaxMemory (LShiftU64 (Length, 1)) - Length;
   Subtractive++;
   do {
     TempQword -= Power2MaxMemory (TempQword);
@@ -625,10 +655,10 @@ GetDirection (
   } while (TempQword != 0);
 
   if (Positive <= Subtractive) {
-    *MtrrNumber = Positive;
+    *MtrrNumber += Positive;
     return TRUE;
   } else {
-    *MtrrNumber = Subtractive;
+    *MtrrNumber += Subtractive;
     return FALSE;
   }
 }
@@ -887,7 +917,7 @@ MtrrSetMemoryAttribute (
   UINT64                    TempQword;
   RETURN_STATUS             Status;
   UINT64                    MemoryType;
-  UINT64                    Remainder;
+  UINT64                    Alignment;
   BOOLEAN                   OverLap;
   BOOLEAN                   Positive;
   UINT32                    MsrNum;
@@ -964,18 +994,6 @@ MtrrSetMemoryAttribute (
     Length += SIZE_1MB;
   }
 
-  //
-  // Check memory base address alignment
-  //
-  DivU64x64Remainder (BaseAddress, Power2MaxMemory (LShiftU64 (Length, 1)), &Remainder);
-  if (Remainder != 0) {
-    DivU64x64Remainder (BaseAddress, Power2MaxMemory (Length), &Remainder);
-    if (Remainder != 0) {
-      Status = RETURN_UNSUPPORTED;
-      goto Done;
-    }
-  }
-
   //
   // Check for overlap
   //
@@ -1018,22 +1036,75 @@ MtrrSetMemoryAttribute (
     goto Done;
   }
 
+  Positive = GetMtrrNumberAndDirection (BaseAddress, Length, &MtrrNumber);
+
+  if ((UsedMtrr + MtrrNumber) > FirmwareVariableMtrrCount) {
+    Status = RETURN_OUT_OF_RESOURCES;
+    goto Done;
+  }
+
+  //
+  // Invalidate the now-unused MTRRs
+  //
+  InvalidateMtrr(VariableMtrr);
+
+  //
+  // Find first unused MTRR
+  //
+  for (MsrNum = MTRR_LIB_IA32_VARIABLE_MTRR_BASE;
+       MsrNum < VariableMtrrEnd;
+       MsrNum += 2
+      ) {
+    if ((AsmReadMsr64 (MsrNum + 1) & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
+      break;
+    }
+  }
+
+  if (BaseAddress != 0) {
+    do {
+      //
+      // Calculate the alignment of the base address.
+      //
+      Alignment = LShiftU64 (1, (UINTN)LowBitSet64 (BaseAddress));
+
+      if (Alignment > Length) {
+        break;
+      }
+
+      //
+      // Find unused MTRR
+      //
+      for (; MsrNum < VariableMtrrEnd; MsrNum += 2) {
+        if ((AsmReadMsr64 (MsrNum + 1) & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
+          break;
+        }
+      }
+
+      ProgramVariableMtrr (
+        MsrNum,
+        BaseAddress,
+        Alignment,
+        MemoryType,
+        MtrrValidAddressMask
+        );
+      BaseAddress += Alignment;
+      Length -= Alignment;
+    } while (TRUE);
+
+    if (Length == 0) {
+      goto Done;
+    }
+  }
+
   TempQword = Length;
 
-
-  if (TempQword == Power2MaxMemory (TempQword)) {
-    //
-    // Invalidate the now-unused MTRRs
-    //
-    InvalidateMtrr(VariableMtrr);
+  if (!Positive) {
+    Length = Power2MaxMemory (LShiftU64 (TempQword, 1));
 
     //
-    // Find first unused MTRR
+    // Find unused MTRR
     //
-    for (MsrNum = MTRR_LIB_IA32_VARIABLE_MTRR_BASE;
-         MsrNum < VariableMtrrEnd;
-         MsrNum += 2
-        ) {
+    for (; MsrNum < VariableMtrrEnd; MsrNum += 2) {
       if ((AsmReadMsr64 (MsrNum + 1) & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
         break;
       }
@@ -1046,76 +1117,40 @@ MtrrSetMemoryAttribute (
       MemoryType,
       MtrrValidAddressMask
       );
-  } else {
-
-    Positive = GetDirection (TempQword, &MtrrNumber);
-
-    if ((UsedMtrr + MtrrNumber) > FirmwareVariableMtrrCount) {
-      Status = RETURN_OUT_OF_RESOURCES;
-      goto Done;
-    }
+    BaseAddress += Length;
+    TempQword   = Length - TempQword;
+    MemoryType  = MTRR_CACHE_UNCACHEABLE;
+  }
 
+  do {
     //
-    // Invalidate the now-unused MTRRs
+    // Find unused MTRR
     //
-    InvalidateMtrr(VariableMtrr);
-
-    //
-    // Find first unused MTRR
-    //
-    for (MsrNum = MTRR_LIB_IA32_VARIABLE_MTRR_BASE;
-         MsrNum < VariableMtrrEnd;
-         MsrNum += 2
-        ) {
+    for (; MsrNum < VariableMtrrEnd; MsrNum += 2) {
       if ((AsmReadMsr64 (MsrNum + 1) & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
         break;
       }
     }
 
+    Length = Power2MaxMemory (TempQword);
     if (!Positive) {
-      Length = Power2MaxMemory (LShiftU64 (TempQword, 1));
-      ProgramVariableMtrr (
-        MsrNum,
-        BaseAddress,
-        Length,
-        MemoryType,
-        MtrrValidAddressMask
-        );
-      BaseAddress += Length;
-      TempQword   = Length - TempQword;
-      MemoryType  = MTRR_CACHE_UNCACHEABLE;
+      BaseAddress -= Length;
     }
 
-    do {
-      //
-      // Find unused MTRR
-      //
-      for (; MsrNum < VariableMtrrEnd; MsrNum += 2) {
-        if ((AsmReadMsr64 (MsrNum + 1) & MTRR_LIB_CACHE_MTRR_ENABLED) == 0) {
-          break;
-        }
-      }
+    ProgramVariableMtrr (
+      MsrNum,
+      BaseAddress,
+      Length,
+      MemoryType,
+      MtrrValidAddressMask
+      );
 
-      Length = Power2MaxMemory (TempQword);
-      if (!Positive) {
-        BaseAddress -= Length;
-      }
+    if (Positive) {
+      BaseAddress += Length;
+    }
+    TempQword -= Length;
 
-      ProgramVariableMtrr (
-        MsrNum,
-        BaseAddress,
-        Length,
-        MemoryType,
-        MtrrValidAddressMask
-        );
-
-      if (Positive) {
-        BaseAddress += Length;
-      }
-      TempQword -= Length;
-
-    } while (TempQword > 0);
-  }
+  } while (TempQword > 0);
 
 Done:
   DEBUG((DEBUG_CACHE, "  Status = %r\n", Status));