ArmVirtPkg/NorFlashQemuLib: discover NOR flash banks dynamically

[mirror_edk2.git] / Vlv2TbltDevicePkg / PlatformInitPei / MemoryPeim.c

diff --git a/Vlv2TbltDevicePkg/PlatformInitPei/MemoryPeim.c b/Vlv2TbltDevicePkg/PlatformInitPei/MemoryPeim.c
index 99bdeb11f9195ee5b13701e46146ab8ac25154f8..6f37e3b72367921039d7b750c38f5e17af9b0a63 100644 (file)
--- a/Vlv2TbltDevicePkg/PlatformInitPei/MemoryPeim.c
+++ b/Vlv2TbltDevicePkg/PlatformInitPei/MemoryPeim.c
@@ -70,6 +70,34 @@ GetMemorySize (
   );\r
 \r
 \r
+/**\r
+  Initializes the valid address mask for MTRRs.\r
+\r
+  This function initializes the valid bits mask and valid address mask for MTRRs.\r
+\r
+**/\r
+UINT64\r
+InitializeAddressMtrrMask (\r
+  VOID\r
+  )\r
+{\r
+  UINT32                    RegEax;\r
+  UINT8                     PhysicalAddressBits; \r
+  UINT64                    ValidMtrrBitsMask;\r
+\r
+  AsmCpuid (0x80000000, &RegEax, NULL, NULL, NULL);\r
+\r
+  if (RegEax >= 0x80000008) {\r
+    AsmCpuid (0x80000008, &RegEax, NULL, NULL, NULL);\r
+\r
+    PhysicalAddressBits = (UINT8) RegEax;\r
+  } else {\r
+    PhysicalAddressBits = 36;\r
+  }\r
+\r
+  ValidMtrrBitsMask    = LShiftU64 (1, PhysicalAddressBits) - 1;\r
+  return (ValidMtrrBitsMask & 0xfffffffffffff000ULL);\r
+}\r
 \r
 EFI_STATUS\r
 EFIAPI\r
@@ -89,6 +117,7 @@ SetPeiCacheMode (
   UINT64                  HighMemoryLength;\r
   UINT8                   Index;\r
   MTRR_SETTINGS           MtrrSetting;\r
+  UINT64                  ValidMtrrAddressMask;\r
 \r
   //\r
   // Load Cache PPI\r
@@ -124,6 +153,8 @@ SetPeiCacheMode (
                              &BootMode\r
                              );\r
 \r
+  ValidMtrrAddressMask = InitializeAddressMtrrMask ();\r
+\r
   //\r
   // Determine memory usage\r
   //\r
@@ -165,16 +196,22 @@ SetPeiCacheMode (
   // Cache the flash area to improve the boot performance in PEI phase\r
   //\r
   Index = 0;\r
-  MtrrSetting.Variables.Mtrr[0].Base = (FixedPcdGet32 (PcdFlashAreaBaseAddress) | CacheWriteProtected);\r
-  MtrrSetting.Variables.Mtrr[0].Mask = ((~((UINT64)(FixedPcdGet32 (PcdFlashAreaSize) - 1))) & MTRR_LIB_CACHE_VALID_ADDRESS) | MTRR_LIB_CACHE_MTRR_ENABLED;\r
+  ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[0].Base)->Uint64 = FixedPcdGet32 (PcdFlashAreaBaseAddress);\r
+  ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[0].Base)->Bits.Type = CacheWriteProtected;\r
+  ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[0].Mask)->Uint64 = (~((UINT64)(FixedPcdGet32 (PcdFlashAreaSize) - 1))) & ValidMtrrAddressMask;\r
+  ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[0].Mask)->Bits.V = 1;\r
+\r
   Index ++;\r
 \r
   MemOverflow =0;\r
   while (MaxMemoryLength > MemOverflow){\r
-    MtrrSetting.Variables.Mtrr[Index].Base = (MemOverflow & MTRR_LIB_CACHE_VALID_ADDRESS) | CacheWriteBack;\r
     MemoryLength = MaxMemoryLength - MemOverflow;\r
     MemoryLength = GetPowerOfTwo64 (MemoryLength);\r
-    MtrrSetting.Variables.Mtrr[Index].Mask = ((~(MemoryLength - 1)) & MTRR_LIB_CACHE_VALID_ADDRESS) | MTRR_LIB_CACHE_MTRR_ENABLED;\r
+\r
+    ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Base)->Uint64 = MemOverflow & ValidMtrrAddressMask;\r
+    ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Base)->Bits.Type = CacheWriteBack;\r
+    ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Mask)->Uint64 = (~(MemoryLength - 1)) & ValidMtrrAddressMask;\r
+    ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Mask)->Bits.V = 1;\r
 \r
     MemOverflow += MemoryLength;\r
     Index++;\r
@@ -185,23 +222,28 @@ SetPeiCacheMode (
   while (MaxMemoryLength != MemoryLength) {\r
     MemoryLengthUc = GetPowerOfTwo64 (MaxMemoryLength - MemoryLength);\r
 \r
-    MtrrSetting.Variables.Mtrr[Index].Base = ((MaxMemoryLength - MemoryLengthUc) & MTRR_LIB_CACHE_VALID_ADDRESS) | CacheUncacheable;\r
-    MtrrSetting.Variables.Mtrr[Index].Mask= ((~(MemoryLengthUc   - 1)) & MTRR_LIB_CACHE_VALID_ADDRESS) | MTRR_LIB_CACHE_MTRR_ENABLED;\r
+    ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Base)->Uint64 = (MaxMemoryLength - MemoryLengthUc) & ValidMtrrAddressMask;\r
+    ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Base)->Bits.Type = CacheUncacheable;\r
+    ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Mask)->Uint64 = (~(MemoryLengthUc   - 1)) & ValidMtrrAddressMask;\r
+    ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Mask)->Bits.V = 1;\r
+\r
     MaxMemoryLength -= MemoryLengthUc;\r
     Index++;\r
   }\r
 \r
   MemOverflow =0x100000000;\r
   while (HighMemoryLength > 0) {\r
-    MtrrSetting.Variables.Mtrr[Index].Base = (MemOverflow & MTRR_LIB_CACHE_VALID_ADDRESS) | CacheWriteBack;\r
+\r
     MemoryLength = HighMemoryLength;\r
     MemoryLength = GetPowerOfTwo64 (MemoryLength);\r
-\r
     if (MemoryLength > MemOverflow){\r
       MemoryLength = MemOverflow;\r
     }\r
 \r
-    MtrrSetting.Variables.Mtrr[Index].Mask = ((~(MemoryLength - 1)) & MTRR_LIB_CACHE_VALID_ADDRESS) | MTRR_LIB_CACHE_MTRR_ENABLED;\r
+    ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Base)->Uint64 = MemOverflow & ValidMtrrAddressMask;\r
+    ((MSR_IA32_MTRR_PHYSBASE_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Base)->Bits.Type = CacheWriteBack;\r
+    ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Mask)->Uint64 = (~(MemoryLength - 1)) & ValidMtrrAddressMask;\r
+    ((MSR_IA32_MTRR_PHYSMASK_REGISTER *) &MtrrSetting.Variables.Mtrr[Index].Mask)->Bits.V = 1;\r
 \r
     MemOverflow += MemoryLength;\r
     HighMemoryLength -= MemoryLength;\r