ArmPlatformPkg/NorFlashDxe: prepare for devicepath format change

[mirror_edk2.git] / ArmPlatformPkg / Drivers / NorFlashDxe / NorFlashDxe.c

diff --git a/ArmPlatformPkg/Drivers/NorFlashDxe/NorFlashDxe.c b/ArmPlatformPkg/Drivers/NorFlashDxe/NorFlashDxe.c
index 3abbe5cb32bc2c7f44af3f014e6cb4515e022ff6..53753a4721ac980f202e690b596a1ad31e7250c6 100644 (file)
--- a/ArmPlatformPkg/Drivers/NorFlashDxe/NorFlashDxe.c
+++ b/ArmPlatformPkg/Drivers/NorFlashDxe/NorFlashDxe.c
@@ -32,9 +32,6 @@ NOR_FLASH_INSTANCE  mNorFlashInstanceTemplate = {
   NOR_FLASH_SIGNATURE, // Signature\r
   NULL, // Handle ... NEED TO BE FILLED\r
 \r
-  FALSE, // Initialized\r
-  NULL, // Initialize\r
-\r
   0, // DeviceBaseAddress ... NEED TO BE FILLED\r
   0, // RegionBaseAddress ... NEED TO BE FILLED\r
   0, // Size ... NEED TO BE FILLED\r
@@ -69,7 +66,6 @@ NOR_FLASH_INSTANCE  mNorFlashInstanceTemplate = {
     NorFlashDiskIoWriteDisk        // WriteDisk\r
   },\r
 \r
-  FALSE, // SupportFvb ... NEED TO BE FILLED\r
   {\r
     FvbGetAttributes, // GetAttributes\r
     FvbSetAttributes, // SetAttributes\r
@@ -86,7 +82,10 @@ NOR_FLASH_INSTANCE  mNorFlashInstanceTemplate = {
       {\r
         HARDWARE_DEVICE_PATH,\r
         HW_VENDOR_DP,\r
-        { (UINT8)sizeof(VENDOR_DEVICE_PATH), (UINT8)((sizeof(VENDOR_DEVICE_PATH)) >> 8) }\r
+        {\r
+          (UINT8)(OFFSET_OF (NOR_FLASH_DEVICE_PATH, End)),\r
+          (UINT8)(OFFSET_OF (NOR_FLASH_DEVICE_PATH, End) >> 8)\r
+        }\r
       },\r
       { 0x0, 0x0, 0x0, { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 } }, // GUID ... NEED TO BE FILLED\r
     },\r
@@ -103,7 +102,7 @@ NorFlashCreateInstance (
   IN UINTN                  NorFlashDeviceBase,\r
   IN UINTN                  NorFlashRegionBase,\r
   IN UINTN                  NorFlashSize,\r
-  IN UINT32                 MediaId,\r
+  IN UINT32                 Index,\r
   IN UINT32                 BlockSize,\r
   IN BOOLEAN                SupportFvb,\r
   IN CONST GUID             *NorFlashGuid,\r
@@ -125,7 +124,7 @@ NorFlashCreateInstance (
   Instance->Size = NorFlashSize;\r
 \r
   Instance->BlockIoProtocol.Media = &Instance->Media;\r
-  Instance->Media.MediaId = MediaId;\r
+  Instance->Media.MediaId = Index;\r
   Instance->Media.BlockSize = BlockSize;\r
   Instance->Media.LastBlock = (NorFlashSize / BlockSize)-1;\r
 \r
@@ -137,8 +136,7 @@ NorFlashCreateInstance (
   }\r
 \r
   if (SupportFvb) {\r
-    Instance->SupportFvb = TRUE;\r
-    Instance->Initialize = NorFlashFvbInitialize;\r
+    NorFlashFvbInitialize (Instance);\r
 \r
     Status = gBS->InstallMultipleProtocolInterfaces (\r
                   &Instance->Handle,\r
@@ -152,8 +150,6 @@ NorFlashCreateInstance (
       return Status;\r
     }\r
   } else {\r
-    Instance->Initialized = TRUE;\r
-\r
     Status = gBS->InstallMultipleProtocolInterfaces (\r
                     &Instance->Handle,\r
                     &gEfiDevicePathProtocolGuid, &Instance->DevicePath,\r
@@ -744,6 +740,65 @@ NorFlashWriteBlocks (
   return Status;\r
 }\r
 \r
+#define BOTH_ALIGNED(a, b, align) ((((UINTN)(a) | (UINTN)(b)) & ((align) - 1)) == 0)\r
+\r
+/**\r
+  Copy Length bytes from Source to Destination, using aligned accesses only.\r
+  Note that this implementation uses memcpy() semantics rather then memmove()\r
+  semantics, i.e., SourceBuffer and DestinationBuffer should not overlap.\r
+\r
+  @param  DestinationBuffer The target of the copy request.\r
+  @param  SourceBuffer      The place to copy from.\r
+  @param  Length            The number of bytes to copy.\r
+\r
+  @return Destination\r
+\r
+**/\r
+STATIC\r
+VOID *\r
+AlignedCopyMem (\r
+  OUT     VOID                      *DestinationBuffer,\r
+  IN      CONST VOID                *SourceBuffer,\r
+  IN      UINTN                     Length\r
+  )\r
+{\r
+  UINT8             *Destination8;\r
+  CONST UINT8       *Source8;\r
+  UINT32            *Destination32;\r
+  CONST UINT32      *Source32;\r
+  UINT64            *Destination64;\r
+  CONST UINT64      *Source64;\r
+\r
+  if (BOTH_ALIGNED(DestinationBuffer, SourceBuffer, 8) && Length >= 8) {\r
+    Destination64 = DestinationBuffer;\r
+    Source64 = SourceBuffer;\r
+    while (Length >= 8) {\r
+      *Destination64++ = *Source64++;\r
+      Length -= 8;\r
+    }\r
+\r
+    Destination8 = (UINT8 *)Destination64;\r
+    Source8 = (CONST UINT8 *)Source64;\r
+  } else if (BOTH_ALIGNED(DestinationBuffer, SourceBuffer, 4) && Length >= 4) {\r
+    Destination32 = DestinationBuffer;\r
+    Source32 = SourceBuffer;\r
+    while (Length >= 4) {\r
+      *Destination32++ = *Source32++;\r
+      Length -= 4;\r
+    }\r
+\r
+    Destination8 = (UINT8 *)Destination32;\r
+    Source8 = (CONST UINT8 *)Source32;\r
+  } else {\r
+    Destination8 = DestinationBuffer;\r
+    Source8 = SourceBuffer;\r
+  }\r
+  while (Length-- != 0) {\r
+    *Destination8++ = *Source8++;\r
+  }\r
+  return DestinationBuffer;\r
+}\r
+\r
 EFI_STATUS\r
 NorFlashReadBlocks (\r
   IN NOR_FLASH_INSTANCE   *Instance,\r
@@ -791,7 +846,7 @@ NorFlashReadBlocks (
   SEND_NOR_COMMAND (Instance->DeviceBaseAddress, 0, P30_CMD_READ_ARRAY);\r
 \r
   // Readout the data\r
-  CopyMem(Buffer, (UINTN *)StartAddress, BufferSizeInBytes);\r
+  AlignedCopyMem (Buffer, (VOID *)StartAddress, BufferSizeInBytes);\r
 \r
   return EFI_SUCCESS;\r
 }\r
@@ -832,7 +887,7 @@ NorFlashRead (
   SEND_NOR_COMMAND (Instance->DeviceBaseAddress, 0, P30_CMD_READ_ARRAY);\r
 \r
   // Readout the data\r
-  CopyMem (Buffer, (UINTN *)(StartAddress + Offset), BufferSizeInBytes);\r
+  AlignedCopyMem (Buffer, (VOID *)(StartAddress + Offset), BufferSizeInBytes);\r
 \r
   return EFI_SUCCESS;\r
 }\r
@@ -865,10 +920,6 @@ NorFlashWriteSingleBlock (
 \r
   PrevBlockAddress = 0;\r
 \r
-  if (!Instance->Initialized && Instance->Initialize) {\r
-    Instance->Initialize(Instance);\r
-  }\r
-\r
   DEBUG ((DEBUG_BLKIO, "NorFlashWriteSingleBlock(Parameters: Lba=%ld, Offset=0x%x, *NumBytes=0x%x, Buffer @ 0x%08x)\n", Lba, Offset, *NumBytes, Buffer));\r
 \r
   // Detect WriteDisabled state\r