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

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