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

1 files 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;

@@ -965,18 +995,6 @@ MtrrSetMemoryAttribute (
   }

 

   //

-  // 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

   //

   UsedMtrr = MtrrGetMemoryAttributeInVariableMtrr (MtrrValidBitsMask, MtrrValidAddressMask, VariableMtrr);

@@ -1018,22 +1036,75 @@ MtrrSetMemoryAttribute (
     goto Done;

   }

 

-  TempQword = Length;

+  Positive = GetMtrrNumberAndDirection (BaseAddress, Length, &MtrrNumber);

 

+  if ((UsedMtrr + MtrrNumber) > FirmwareVariableMtrrCount) {

+    Status = RETURN_OUT_OF_RESOURCES;

+    goto Done;

+  }

 

-  if (TempQword == Power2MaxMemory (TempQword)) {

-    //

-    // Invalidate the now-unused MTRRs

-    //

-    InvalidateMtrr(VariableMtrr);

+  //

+  // 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 (!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;

-    }

-

-    //

-    // Invalidate the now-unused MTRRs

-    //

-    InvalidateMtrr(VariableMtrr);

+    BaseAddress += Length;

+    TempQword   = Length - TempQword;

+    MemoryType  = MTRR_CACHE_UNCACHEABLE;

+  }

 

+  do {

     //

-    // 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;

       }

     }

 

+    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;

-        }

-      }

-

-      Length = Power2MaxMemory (TempQword);

-      if (!Positive) {

-        BaseAddress -= Length;

-      }

-

-      ProgramVariableMtrr (

-        MsrNum,

-        BaseAddress,

-        Length,

-        MemoryType,

-        MtrrValidAddressMask

-        );

+    ProgramVariableMtrr (

+      MsrNum,

+      BaseAddress,

+      Length,

+      MemoryType,

+      MtrrValidAddressMask

+      );

 

-      if (Positive) {

-        BaseAddress += Length;

-      }

-      TempQword -= Length;

+    if (Positive) {

+      BaseAddress += Length;

+    }

+    TempQword -= Length;

 

-    } while (TempQword > 0);

-  }

+  } while (TempQword > 0);

 

 Done:

   DEBUG((DEBUG_CACHE, "  Status = %r\n", Status));