diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.dec b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.dec
new file mode 100644
index 000000000000..aec95bc82387
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.dec
@@ -0,0 +1,31 @@
+## @file
+#  Socionext FIP006 High-Speed SPI Controller with NOR Flash Driver
+#
+#  Copyright (c) 2017, Socionext Inc. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+[Defines]
+  DEC_SPECIFICATION             = 0x0001001A
+  PACKAGE_NAME                  = Fip006DxePkg
+  PACKAGE_GUID                  = ABC7870B-FE82-4DAD-8179-FEC5F5194FA0
+  PACKAGE_VERSION               = 0.1
+
+[Guids]
+  gFip006DxeTokenSpaceGuid      = {0x4D45399E, 0x98F9, 0x4127, {0x8F, 0xB9,0xF8, 0xDE, 0x22, 0xA1, 0x09, 0x2C}}
+
+[PcdsFixedAtBuild]
+  gFip006DxeTokenSpaceGuid.PcdFip006DxeRegBaseAddress|0x0|UINT32|0x00000001
+  gFip006DxeTokenSpaceGuid.PcdFip006DxeMemBaseAddress|0x0|UINT32|0x00000002
+  gFip006DxeTokenSpaceGuid.PcdN25qSlaveId|0x0|UINT8|0x00000003
+  gFip006DxeTokenSpaceGuid.PcdN25qBlockSize|256|UINT32|0x00000004
+  gFip006DxeTokenSpaceGuid.PcdN25qBlockCount|524288|UINT32|0x00000005
+
diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.inf b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.inf
new file mode 100644
index 000000000000..62f81cfe33cd
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.inf
@@ -0,0 +1,81 @@
+## @file
+#  Socionext FIP006 High-Speed SPI Controller with NOR Flash Driver
+#
+#  Copyright (c) 2017, Socionext Inc. All rights reserved.<BR>
+#  Copyright (c) 2017, Linaro, Ltd. All rights reserved.<BR>
+#
+#  This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution.  The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+##
+
+
+[Defines]
+  INF_VERSION                    = 0x0001001A
+  BASE_NAME                      = Fip006Dxe
+  FILE_GUID                      = 44F7D21F-C36F-4766-BC5B-C72E97E6897B
+  MODULE_TYPE                    = DXE_RUNTIME_DRIVER
+  VERSION_STRING                 = 0.1
+  ENTRY_POINT                    = NorFlashInitialise
+
+[Sources]
+  NorFlashBlockIoDxe.c
+  NorFlashDxe.c
+  NorFlashFvbDxe.c
+
+[Packages]
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+  Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Dxe.dec
+
+[LibraryClasses]
+  BaseLib
+  BaseMemoryLib
+  DebugLib
+  DevicePathLib
+  DxeServicesTableLib
+  HobLib
+  IoLib
+  MemoryAllocationLib
+  NorFlashInfoLib
+  NorFlashPlatformLib
+  UefiBootServicesTableLib
+  UefiDriverEntryPoint
+  UefiLib
+  UefiRuntimeLib
+  UefiRuntimeServicesTableLib
+
+[Guids]
+  gEfiAuthenticatedVariableGuid
+  gEfiEventVirtualAddressChangeGuid
+  gEfiSystemNvDataFvGuid
+  gEfiVariableGuid
+
+[Protocols]
+  gEfiBlockIoProtocolGuid
+  gEfiDevicePathProtocolGuid
+  gEfiDiskIoProtocolGuid
+  gEfiFirmwareVolumeBlockProtocolGuid
+
+[FixedPcd]
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableBase
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageVariableSize
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingBase
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwWorkingSize
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareBase
+  gEfiMdeModulePkgTokenSpaceGuid.PcdFlashNvStorageFtwSpareSize
+  gFip006DxeTokenSpaceGuid.PcdFip006DxeRegBaseAddress
+  gFip006DxeTokenSpaceGuid.PcdFip006DxeMemBaseAddress
+
+[Depex]
+  #
+  # NorFlashDxe must be loaded before VariableRuntimeDxe in case empty flash needs populating with default values
+  #
+  BEFORE gVariableRuntimeDxeFileGuid
diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Reg.h b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Reg.h
new file mode 100644
index 000000000000..3d758dc492f6
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/Fip006Reg.h
@@ -0,0 +1,244 @@
+/** @file
+  Socionext FIP006 Register List
+
+  Copyright (c) 2017, Socionext Inc. All rights reserved.<BR>
+  Copyright (c) 2017, Linaro, Ltd. All rights reserved.<BR>
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef __EFI_FIP006_REG_H__
+#define __EFI_FIP006_REG_H__
+
+#define FIP006_REG_MCTRL      0x00    // Module Control
+typedef union {
+  UINT32          Raw       : 32;
+  struct {
+    BOOLEAN       MEN       : 1;
+    BOOLEAN       CSEN      : 1;
+#define MCTRL_CSEN_DIRECT     0
+#define MCTRL_CSEN_CS         1
+    BOOLEAN       CDSS      : 1;
+#define MCTRL_CDSS_IHCLK      0
+#define MCTRL_CDSS_IPCLK      1
+    BOOLEAN       MES       : 1;
+#define MCTRL_MES_READY       1
+    UINT8                   : 4;
+    UINT8                   : 8;
+    UINT8                   : 8;
+    UINT8                   : 8;
+  };
+} FIP006_MCTRL;
+
+#define FIP006_REG_PCC0       0x04    // Peripheral Communication Control 0
+#define FIP006_REG_PCC1       0x08    // Peripheral Communication Control 1
+#define FIP006_REG_PCC2       0x0C    // Peripheral Communication Control 2
+#define FIP006_REG_PCC3       0x10    // Peripheral Communication Control 3
+typedef union {
+  UINT32          Raw       : 32;
+  struct {
+    BOOLEAN       CPHA      : 1;
+    BOOLEAN       CPOL      : 1;
+    BOOLEAN       ACES      : 1;
+    BOOLEAN       RTM       : 1;
+    BOOLEAN       SSPOL     : 1;
+    UINT8         SS2CD     : 2;
+    BOOLEAN       SDIR      : 1;
+#define PCC_SDIR_MS_BIT       0
+#define PCC_SDIR_LS_BIT       1
+    BOOLEAN       SENDIAN   : 1;
+#define PCC_SENDIAN_BIG       0
+#define PCC_SENDIAN_LITTLE    1
+    UINT8         CDRS      : 7;
+    BOOLEAN       SAFESYNC  : 1;
+    UINT8         WRDSEL    : 4;
+    UINT8         RDDSEL    : 2;
+    UINT8                   : 1;
+    UINT8                   : 8;
+  } Reg;
+} FIP006_PCC;
+typedef FIP006_PCC          FIP006_PCC0, FIP006_PCC1, FIP006_PCC2, FIP006_PCC3;
+
+#define FIP006_REG_TXF        0x14    // Tx Interrupt Flag
+#define TXF_TSSRS             BIT6
+#define TXF_TFMTS             BIT5
+#define TXF_TFLETS            BIT4
+#define TXF_TFUS              BIT3
+#define TXF_TFOS              BIT2
+#define TXF_TFES              BIT1
+#define TXF_TFFS              BIT0
+
+#define FIP006_REG_TXE        0x18    // Tx Interrupt Enable
+#define TXE_TSSRE             BIT6
+#define TXE_TFMTE             BIT5
+#define TXE_TFLETE            BIT4
+#define TXE_TFUE              BIT3
+#define TXE_TFOE              BIT2
+#define TXE_TFEE              BIT1
+#define TXE_TFFE              BIT0
+
+#define FIP006_REG_TXC        0x1C    // Tx Interrupt Clear
+#define TXC_TSSRC             BIT6
+#define TXC_TFMTC             BIT5
+#define TXC_TFLETC            BIT4
+#define TXC_TFUC              BIT3
+#define TXC_TFOC              BIT2
+#define TXC_TFEC              BIT1
+#define TXC_TFFC              BIT0
+
+#define FIP006_REG_RXF        0x20    // Rx Interrupt Flag
+#define RXF_RSSRS             BIT6
+#define RXF_RFMTS             BIT5
+#define RXF_RFLETS            BIT4
+#define RXF_RFUS              BIT3
+#define RXF_RFOS              BIT2
+#define RXF_RFES              BIT1
+#define RXF_RFFS              BIT0
+
+#define FIP006_REG_RXE        0x24    // Rx Interrupt Enable
+#define RXE_RSSRE             BIT6
+#define RXE_RFMTE             BIT5
+#define RXE_RFLETE            BIT4
+#define RXE_RFUE              BIT3
+#define RXE_RFOE              BIT2
+#define RXE_RFEE              BIT1
+#define RXE_RFFE              BIT0
+
+#define FIP006_REG_RXC        0x28    // Rx Interrupt Clear
+#define RXC_RSSRC             BIT6
+#define RXC_RFMTC             BIT5
+#define RXC_RFLETC            BIT4
+#define RXC_RFUC              BIT3
+#define RXC_RFOC              BIT2
+#define RXC_RFEC              BIT1
+#define RXC_RFFC              BIT0
+
+#define FIP006_REG_FAULTF     0x2C    // Error Interrupt Status
+#define FAULTF_DRCBSFS        BIT4
+#define FAULTF_DWCBSFS        BIT3
+#define FAULTF_PVFS           BIT2
+#define FAULTF_WAFS           BIT1
+#define FAULTF_UMAFS          BIT0
+
+#define FIP006_REG_FAULTC     0x30    // Error Interrupt Clear
+#define FAULTC_DRCBSFC        BIT4
+#define FAULTC_DWCBSFC        BIT3
+#define FAULTC_PVFC           BIT2
+#define FAULTC_WAFC           BIT1
+#define FAULTC_UMAFC          BIT0
+
+#define FIP006_REG_DM_CFG     0x34    // Direct Mode DMA Configuration
+#define DM_CFG_MSTARTEN       BIT2
+#define DM_CFG_SSDC           BIT1
+
+#define FIP006_REG_DM_DMA     0x35    // Direct Mode DMA Enable
+#define DM_DMA_TXDMAEN        BIT1
+#define DM_DMA_RXDMAEN        BIT0
+
+#define FIP006_REG_DM_START   0x38    // Direct Mode Start Transmission
+#define DM_START              BIT0
+#define FIP006_REG_DM_STOP    0x39    // Direct Mode Stop Transmission
+#define DM_STOP               BIT0
+
+#define FIP006_REG_DM_PSEL    0x3A    // Direct Mode Peripheral Select
+#define DM_PSEL               (BIT1 | BIT0)
+
+#define FIP006_REG_DM_TRP     0x3B    // Direct Mode Transmission Protocol
+#define DM_TRP                (BIT3 | BIT2 | BIT1 | BIT0)
+
+#define FIP006_REG_DM_BCC     0x3C    // Direct Mode Byte Count Control
+#define FIP006_REG_DM_BCS     0x3E    // Direct Mode Byte Count Status
+
+#define FIP006_REG_DM_STATUS  0x40    // Direct Mode Status
+#define DM_STATUS_TXFLEVEL    (BIT20 | BIT19 | BIT18 | BIT17 | BIT16)
+#define DM_STATUS_RXFLEVEL    (BIT20 | BIT19 | BIT18 | BIT17 | BIT16)
+#define DM_STATUS_TXACTIVE    BIT1
+#define DM_STATUS_RXACTIVE    BIT0
+
+#define FIP006_REG_FIFO_CFG   0x4C    // FIFO Configuration
+#define FIFO_CFG_TXFLSH       BIT12
+#define FIFO_CFG_RXFLSH       BIT11
+#define FIFO_CFG_TXCTRL       BIT10
+#define FIFO_CFG_FWIDTH       (BIT9 | BIT8)
+#define FIFO_CFG_TXFTH        (BIT7 | BIT6 | BIT5 | BIT4)
+#define FIFO_CFG_RXFTH        (BIT3 | BIT2 | BIT1 | BIT0)
+
+#define FIP006_REG_FIFO_TX    0x50    // 16 32-bit Tx FIFO
+#define FIP006_REG_FIFO_RX    0x90    // 16 32-bit Rx FIFO
+
+#define FIP006_REG_CS_CFG     0xD0    // Command Sequencer Configuration
+typedef union {
+  UINT32          Raw       : 32;
+  struct {
+    BOOLEAN       SRAM      : 1;
+#define CS_CFG_SRAM_RO        0
+#define CS_CFG_SRAM_RW        1
+    UINT8         MBM       : 2;
+#define CS_CFG_MBM_SINGLE     0
+#define CS_CFG_MBM_DUAL       1
+#define CS_CFG_MBM_QUAD       2
+    BOOLEAN       SPICHNG   : 1;
+    BOOLEAN       BOOTEN    : 1;
+    BOOLEAN       BSEL      : 1;
+    UINT8                   : 2;
+    BOOLEAN       SSEL0EN   : 1;
+    BOOLEAN       SSEL1EN   : 1;
+    BOOLEAN       SSEL2EN   : 1;
+    BOOLEAN       SSEL3EN   : 1;
+    UINT8                   : 4;
+    BOOLEAN       MSEL      : 4;
+    UINT8                   : 4;
+    UINT8                   : 8;
+  } Reg;
+} FIP006_CS_CFG;
+
+#define FIP006_REG_CS_ITIME   0xD4    // Command Sequencer Idle Timer
+typedef union {
+  UINT32          Raw       : 32;
+  struct {
+    UINT16        ITIME     : 16;
+    UINT16                  : 16;
+  } Reg;
+} FIP006_CS_ITIME;
+#define FIP006_REG_CS_AEXT    0xD8    // Command Sequencer Address Extension
+typedef union {
+  UINT32          Raw       : 32;
+  struct {
+    UINT16                  : 13;
+    UINT32        AEXT      : 19;
+  } Reg;
+} FIP006_CS_AEXT;
+
+#define FIP006_REG_CS_RD      0xDC    // Command Sequencer Read Control
+#define CS_RD_DEPTH           8
+#define FIP006_REG_CS_WR      0xEC    // Command Sequencer Write Control
+#define CS_WR_DEPTH           8
+typedef union {
+  UINT16          Raw       : 16;
+  struct {
+    BOOLEAN       DEC       : 1;
+    UINT8         TRP       : 2;
+    BOOLEAN       CONT      : 1;
+    UINT8                   : 4;
+    union {
+      UINT8       Data      : 8;
+      struct {
+        UINT8     Data      : 3;
+        UINT8               : 5;
+      } Cmd;
+    } Payload;
+  } Reg;
+} FIP006_CS_CMD;
+typedef FIP006_CS_CMD       FIP006_CS_RD, FIP006_CS_WR;
+
+#define FIP006_REG_MID        0xFC    // Command Sequencer Module ID
+typedef UINT32    FIP006_MID;
+
+#endif
diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashBlockIoDxe.c b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashBlockIoDxe.c
new file mode 100644
index 000000000000..b41f5003217c
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashBlockIoDxe.c
@@ -0,0 +1,138 @@
+/** @file  NorFlashBlockIoDxe.c
+
+  Copyright (c) 2011-2013, ARM Ltd. All rights reserved.<BR>
+  Copyright (c) 2011 - 2014, ARM Ltd. All rights reserved.<BR>
+  Copyright (c) 2017, Linaro, Ltd. All rights reserved.<BR>
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include "NorFlashDxe.h"
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.Reset
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoReset (
+  IN EFI_BLOCK_IO_PROTOCOL  *This,
+  IN BOOLEAN                ExtendedVerification
+  )
+{
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_BLKIO_THIS(This);
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashBlockIoReset(MediaId=0x%x)\n",
+    This->Media->MediaId));
+
+  return NorFlashReset (Instance);
+}
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.ReadBlocks
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSizeInBytes,
+  OUT VOID                    *Buffer
+  )
+{
+  NOR_FLASH_INSTANCE  *Instance;
+  EFI_STATUS          Status;
+  EFI_BLOCK_IO_MEDIA  *Media;
+
+  if (This == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Instance = INSTANCE_FROM_BLKIO_THIS(This);
+  Media = This->Media;
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashBlockIoReadBlocks(MediaId=0x%x, Lba=%ld, BufferSize=0x%x bytes "
+    "(%d kB), BufferPtr @ 0x%08x)\n", MediaId, Lba, BufferSizeInBytes, Buffer));
+
+  if (!Media) {
+    Status = EFI_INVALID_PARAMETER;
+  } else if (!Media->MediaPresent) {
+    Status = EFI_NO_MEDIA;
+  } else if (Media->MediaId != MediaId) {
+    Status = EFI_MEDIA_CHANGED;
+  } else if ((Media->IoAlign > 2) &&
+             (((UINTN)Buffer & (Media->IoAlign - 1)) != 0)) {
+    Status = EFI_INVALID_PARAMETER;
+  } else {
+    Status = NorFlashReadBlocks (Instance, Lba, BufferSizeInBytes, Buffer);
+  }
+
+  return Status;
+}
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.WriteBlocks
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSizeInBytes,
+  IN  VOID                    *Buffer
+  )
+{
+  NOR_FLASH_INSTANCE  *Instance;
+  EFI_STATUS          Status;
+
+  Instance = INSTANCE_FROM_BLKIO_THIS(This);
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashBlockIoWriteBlocks(MediaId=0x%x, Lba=%ld, BufferSize=0x%x bytes "
+    "(%d kB), BufferPtr @ 0x%08x)\n", MediaId, Lba, BufferSizeInBytes, Buffer));
+
+  if( !This->Media->MediaPresent ) {
+    Status = EFI_NO_MEDIA;
+  } else if( This->Media->MediaId != MediaId ) {
+    Status = EFI_MEDIA_CHANGED;
+  } else if( This->Media->ReadOnly ) {
+    Status = EFI_WRITE_PROTECTED;
+  } else {
+    Status = NorFlashWriteBlocks (Instance,Lba,BufferSizeInBytes,Buffer);
+  }
+
+  return Status;
+}
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.FlushBlocks
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL  *This
+  )
+{
+  // No Flush required for the NOR Flash driver
+  // because cache operations are not permitted.
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashBlockIoFlushBlocks: Function NOT IMPLEMENTED (not required).\n"));
+
+  // Nothing to do so just return without error
+  return EFI_SUCCESS;
+}
diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashDxe.c b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashDxe.c
new file mode 100644
index 000000000000..a521b1f3d167
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashDxe.c
@@ -0,0 +1,1391 @@
+/** @file  NorFlashDxe.c
+
+  Copyright (c) 2011 - 2014, ARM Ltd. All rights reserved.<BR>
+  Copyright (c) 2017, Socionext Inc. All rights reserved.<BR>
+  Copyright (c) 2017, Linaro, Ltd. All rights reserved.<BR>
+
+  This program and the accompanying materials are licensed and made available
+  under the terms and conditions of the BSD License which accompanies this
+  distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Library/BaseMemoryLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/NorFlashInfoLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+
+#include "NorFlashDxe.h"
+
+STATIC EFI_EVENT mNorFlashVirtualAddrChangeEvent;
+
+//
+// Global variable declarations
+//
+STATIC NOR_FLASH_INSTANCE   **mNorFlashInstances;
+STATIC UINT32               mNorFlashDeviceCount;
+
+STATIC CONST UINT16 mFip006NullCmdSeq[] = {
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+  CSDC (CSDC_END, CSDC_CONT_NON_CONTINUOUS, CSDC_TRP_MBM, CSDC_DEC_DECODE),
+};
+
+STATIC CONST CSDC_DEFINITION mN25qCSDCDefTable[] = {
+  // Identification Operations
+  { SPINOR_OP_RDID,     FALSE, FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  // Register Operations
+  { SPINOR_OP_RDSR,     FALSE, FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_WRSR,     FALSE, FALSE, FALSE, TRUE,  CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_RD_ARRAY, TRUE,  FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_RDFSR,    FALSE, FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_RD_NVCFG, FALSE, FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_RD_VCR,   FALSE, FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_RD_EVCR,  FALSE, FALSE, FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  // Read Operations
+  { SPINOR_OP_READ_4B,  TRUE,  TRUE,  FALSE, FALSE, CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  // Write Operations
+  { SPINOR_OP_PP,       TRUE,  FALSE, FALSE, TRUE,  CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+  { SPINOR_OP_PP_1_1_4, TRUE,  FALSE, FALSE, TRUE,  CS_CFG_MBM_QUAD,
+                        CSDC_TRP_SINGLE },
+  // Erase Operations
+  { SPINOR_OP_SE,       FALSE, FALSE, FALSE, TRUE,  CS_CFG_MBM_SINGLE,
+                        CSDC_TRP_SINGLE },
+};
+
+STATIC
+EFI_STATUS
+NorFlashSetHostCSDC (
+  IN  NOR_FLASH_INSTANCE    *Instance,
+  IN  BOOLEAN               ReadWrite,
+  IN  CONST UINT16          CSDC[ARRAY_SIZE (mFip006NullCmdSeq)]
+  )
+{
+  EFI_PHYSICAL_ADDRESS      Dst;
+  UINTN                     Index;
+
+  Dst = Instance->HostRegisterBaseAddress
+        + (ReadWrite ? FIP006_REG_CS_WR : FIP006_REG_CS_RD);
+  for (Index = 0; Index < ARRAY_SIZE (mFip006NullCmdSeq); Index++) {
+    MmioWrite16 (Dst + (Index << 1), CSDC[Index]);
+  }
+  return EFI_SUCCESS;
+}
+
+STATIC
+CONST CSDC_DEFINITION *
+NorFlashGetCmdDef (
+  IN  NOR_FLASH_INSTANCE    *Instance,
+  IN  UINT8                 Code
+  )
+{
+  CONST CSDC_DEFINITION *Cmd;
+  UINTN                 Index;
+
+  Cmd = NULL;
+  for (Index = 0; Index <  Instance->CmdTableSize; Index++) {
+    if (Code == Instance->CmdTable[Index].Code) {
+      Cmd = &Instance->CmdTable[Index];
+      break;
+    }
+  }
+  return Cmd;
+}
+
+STATIC
+EFI_STATUS
+GenCSDC (
+  IN  UINT8     Cmd,
+  IN  BOOLEAN   AddrAccess,
+  IN  BOOLEAN   AddrMode4Byte,
+  IN  BOOLEAN   HighZ,
+  IN  UINT8     TransferMode,
+  OUT UINT16    *CmdSeq
+  )
+{
+  UINTN         Index;
+
+  if (!CmdSeq) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Index = 0;
+  CopyMem (CmdSeq, mFip006NullCmdSeq, sizeof (mFip006NullCmdSeq));
+
+  CmdSeq[Index++] = CSDC (Cmd, CSDC_CONT_NON_CONTINUOUS, TransferMode,
+                          CSDC_DEC_LEAVE_ASIS);
+  if (AddrAccess) {
+    if (AddrMode4Byte) {
+      CmdSeq[Index++] = CSDC (CSDC_ADDRESS_31_24, CSDC_CONT_NON_CONTINUOUS,
+                              TransferMode, CSDC_DEC_DECODE);
+    }
+    CmdSeq[Index++] = CSDC (CSDC_ADDRESS_23_16, CSDC_CONT_NON_CONTINUOUS,
+                            TransferMode, CSDC_DEC_DECODE);
+    CmdSeq[Index++] = CSDC (CSDC_ADDRESS_15_8, CSDC_CONT_NON_CONTINUOUS,
+                            TransferMode, CSDC_DEC_DECODE);
+    CmdSeq[Index++] = CSDC (CSDC_ADDRESS_7_0, CSDC_CONT_NON_CONTINUOUS,
+                            TransferMode, CSDC_DEC_DECODE);
+  }
+  if (HighZ) {
+    CmdSeq[Index++] = CSDC (CSDC_HIGH_Z, CSDC_CONT_NON_CONTINUOUS,
+                            TransferMode, CSDC_DEC_DECODE);
+  }
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+NorFlashSetHostCommand (
+  IN  NOR_FLASH_INSTANCE    *Instance,
+  IN  UINT8                 Code
+  )
+{
+  CONST CSDC_DEFINITION     *Cmd;
+  UINT16                    CSDC[ARRAY_SIZE (mFip006NullCmdSeq)];
+
+  Cmd = NorFlashGetCmdDef (Instance, Code);
+  if (Cmd == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+  GenCSDC (
+      Cmd->Code,
+      Cmd->AddrAccess,
+      Cmd->AddrMode4Byte,
+      Cmd->HighZ,
+      Cmd->CsdcTrp,
+      CSDC
+      );
+  NorFlashSetHostCSDC (Instance, Cmd->ReadWrite, CSDC);
+  return EFI_SUCCESS;
+}
+
+STATIC
+UINT8
+NorFlashReadStatusRegister (
+  IN NOR_FLASH_INSTANCE     *Instance
+  )
+{
+  UINT8       StatusRegister;
+
+  NorFlashSetHostCommand (Instance, SPINOR_OP_RDSR);
+  StatusRegister = MmioRead8 (Instance->RegionBaseAddress);
+  NorFlashSetHostCommand (Instance, SPINOR_OP_READ_4B);
+  return StatusRegister;
+}
+
+STATIC
+EFI_STATUS
+NorFlashWaitProgramErase (
+  IN NOR_FLASH_INSTANCE     *Instance
+  )
+{
+  BOOLEAN     SRegDone;
+  BOOLEAN     FSRegDone;
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashWaitProgramErase()\n"));
+
+  do {
+    SRegDone = (NorFlashReadStatusRegister (Instance) & SPINOR_SR_WIP) == 0;
+    FSRegDone = TRUE;
+    if (Instance->Flags & NOR_FLASH_POLL_FSR) {
+      NorFlashSetHostCommand (Instance, SPINOR_OP_RDFSR);
+      FSRegDone = (MmioRead8 (Instance->RegionBaseAddress) &
+                   SPINOR_FSR_READY) != 0;
+    }
+  } while (!SRegDone || !FSRegDone);
+  NorFlashSetHostCommand (Instance, SPINOR_OP_READ_4B);
+  return EFI_SUCCESS;
+}
+
+// TODO: implement lock checking
+STATIC
+BOOLEAN
+NorFlashBlockIsLocked (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  BlockAddress
+  )
+{
+  return FALSE;
+}
+
+// TODO: implement sector unlocking
+STATIC
+EFI_STATUS
+NorFlashUnlockSingleBlock (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  BlockAddress
+  )
+{
+  return EFI_SUCCESS;
+}
+
+STATIC
+EFI_STATUS
+NorFlashUnlockSingleBlockIfNecessary (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  BlockAddress
+  )
+{
+  EFI_STATUS Status;
+
+  Status = EFI_SUCCESS;
+
+  if (NorFlashBlockIsLocked (Instance, BlockAddress) == TRUE) {
+    Status = NorFlashUnlockSingleBlock (Instance, BlockAddress);
+  }
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+NorFlashEnableWrite (
+  IN  NOR_FLASH_INSTANCE    *Instance
+  )
+{
+  EFI_STATUS      Status;
+  UINT8           StatusRegister;
+  UINTN           Retry;
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashEnableWrite()\n"));
+
+  Status = EFI_DEVICE_ERROR;
+  Retry = NOR_FLASH_ERASE_RETRY;
+
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+  while (Retry > 0 && EFI_ERROR (Status)) {
+    MmioWrite8 (Instance->RegionBaseAddress, SPINOR_OP_WREN);
+    MemoryFence ();
+    StatusRegister = NorFlashReadStatusRegister (Instance);
+    Status = (StatusRegister & BIT1) ? EFI_SUCCESS : EFI_DEVICE_ERROR;
+    Retry--;
+  }
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+NorFlashDisableWrite (
+  IN  NOR_FLASH_INSTANCE    *Instance
+  )
+{
+  EFI_STATUS      Status;
+  UINT8           StatusRegister;
+  UINTN           Retry;
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashDisableWrite()\n"));
+
+  Status = EFI_DEVICE_ERROR;
+  Retry = NOR_FLASH_ERASE_RETRY;
+
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+  while (Retry > 0 && EFI_ERROR (Status)) {
+    MmioWrite8 (Instance->RegionBaseAddress, SPINOR_OP_WRDIS);
+    MemoryFence ();
+    StatusRegister = NorFlashReadStatusRegister (Instance);
+    Status = (StatusRegister & BIT1) ? EFI_DEVICE_ERROR : EFI_SUCCESS;
+    Retry--;
+  }
+  return Status;
+}
+
+/**
+ * The following function presumes that the block has already been unlocked.
+ **/
+STATIC
+EFI_STATUS
+NorFlashEraseSingleBlock (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  BlockAddress
+  )
+{
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashEraseSingleBlock(BlockAddress=0x%08x)\n",
+    BlockAddress));
+
+  if (EFI_ERROR (NorFlashEnableWrite (Instance))) {
+    return EFI_DEVICE_ERROR;
+  }
+
+  //
+  // The virtual address chosen by the OS may have a different offset modulo
+  // 16 MB than the physical address, so we need to subtract the region base
+  // address before we can mask off a block index. Note that the relative
+  // offset between device base address and region base address may have changed
+  // as well, so we cannot use the device base address directly.
+  //
+  if (EfiAtRuntime()) {
+    BlockAddress -= Instance->RegionBaseAddress;
+    BlockAddress += Instance->OffsetLba * Instance->Media.BlockSize;
+  }
+
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+  MmioWrite32 (Instance->DeviceBaseAddress,
+               SwapBytes32 (BlockAddress & 0x00FFFFFF) | SPINOR_OP_SE);
+  NorFlashWaitProgramErase (Instance);
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+
+  if (EFI_ERROR (NorFlashDisableWrite (Instance))) {
+    return EFI_DEVICE_ERROR;
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+ * This function unlock and erase an entire NOR Flash block.
+ **/
+EFI_STATUS
+NorFlashUnlockAndEraseSingleBlock (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  BlockAddress
+  )
+{
+  EFI_STATUS      Status;
+  UINTN           Index;
+  EFI_TPL         OriginalTPL;
+  BOOLEAN         InterruptsEnabled;
+
+  OriginalTPL = 0;
+  InterruptsEnabled = FALSE;
+
+  if (!EfiAtRuntime ()) {
+    // Raise TPL to TPL_HIGH to stop anyone from interrupting us.
+    OriginalTPL = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  } else {
+    InterruptsEnabled = SaveAndDisableInterrupts ();
+  }
+
+  Index = 0;
+  // The block erase might fail a first time (SW bug ?). Retry it ...
+  do {
+    // Unlock the block if we have to
+    Status = NorFlashUnlockSingleBlockIfNecessary (Instance, BlockAddress);
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+    Status = NorFlashEraseSingleBlock (Instance, BlockAddress);
+    Index++;
+  } while ((Index < NOR_FLASH_ERASE_RETRY) && (Status == EFI_WRITE_PROTECTED));
+
+  if (Index == NOR_FLASH_ERASE_RETRY) {
+    DEBUG ((DEBUG_ERROR,
+      "EraseSingleBlock(BlockAddress=0x%08x: Block Locked Error (try to erase %d times)\n",
+      BlockAddress,Index));
+  }
+
+  if (!EfiAtRuntime ()) {
+    // Interruptions can resume.
+    gBS->RestoreTPL (OriginalTPL);
+  } else if (InterruptsEnabled) {
+    SetInterruptState (TRUE);
+  }
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+NorFlashWriteSingleWord (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  WordAddress,
+  IN UINT32                 WriteData
+  )
+{
+  EFI_STATUS            Status;
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashWriteSingleWord(WordAddress=0x%08x, WriteData=0x%08x)\n",
+    WordAddress, WriteData));
+
+  Status = EFI_SUCCESS;
+
+  if (EFI_ERROR (NorFlashEnableWrite (Instance))) {
+    return EFI_DEVICE_ERROR;
+  }
+  NorFlashSetHostCommand (Instance, SPINOR_OP_PP);
+  MmioWrite32 (WordAddress, WriteData);
+  NorFlashWaitProgramErase (Instance);
+
+  NorFlashDisableWrite (Instance);
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+NorFlashWriteFullBlock (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN EFI_LBA                Lba,
+  IN UINT32                 *DataBuffer,
+  IN UINT32                 BlockSizeInWords
+  )
+{
+  EFI_STATUS    Status;
+  UINTN         WordAddress;
+  UINT32        WordIndex;
+  UINTN         BlockAddress;
+  EFI_TPL       OriginalTPL;
+  BOOLEAN       InterruptsEnabled;
+
+  Status = EFI_SUCCESS;
+  OriginalTPL = 0;
+  InterruptsEnabled = FALSE;
+
+  // Get the physical address of the block
+  BlockAddress = GET_NOR_BLOCK_ADDRESS (Instance->RegionBaseAddress, Lba,
+                   BlockSizeInWords * 4);
+
+  // Start writing from the first address at the start of the block
+  WordAddress = BlockAddress;
+
+  if (!EfiAtRuntime ()) {
+    // Raise TPL to TPL_HIGH to stop anyone from interrupting us.
+    OriginalTPL = gBS->RaiseTPL (TPL_HIGH_LEVEL);
+  } else {
+    InterruptsEnabled = SaveAndDisableInterrupts ();
+  }
+
+  Status = NorFlashUnlockAndEraseSingleBlock (Instance, BlockAddress);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR,
+      "WriteSingleBlock: ERROR - Failed to Unlock and Erase the single block at 0x%X\n",
+      BlockAddress));
+    goto EXIT;
+  }
+
+  for (WordIndex=0;
+       WordIndex < BlockSizeInWords;
+       WordIndex++, DataBuffer++, WordAddress += 4) {
+    Status = NorFlashWriteSingleWord (Instance, WordAddress, *DataBuffer);
+    if (EFI_ERROR (Status)) {
+      goto EXIT;
+    }
+  }
+
+EXIT:
+  if (!EfiAtRuntime ()) {
+    // Interruptions can resume.
+    gBS->RestoreTPL (OriginalTPL);
+  } else if (InterruptsEnabled) {
+    SetInterruptState (TRUE);
+  }
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR,
+      "NOR FLASH Programming [WriteSingleBlock] failed at address 0x%08x. Exit Status = \"%r\".\n",
+      WordAddress, Status));
+  }
+  return Status;
+}
+
+EFI_STATUS
+NorFlashWriteBlocks (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN EFI_LBA                Lba,
+  IN UINTN                  BufferSizeInBytes,
+  IN VOID                   *Buffer
+  )
+{
+  UINT32          *pWriteBuffer;
+  EFI_STATUS      Status = EFI_SUCCESS;
+  EFI_LBA         CurrentBlock;
+  UINT32          BlockSizeInWords;
+  UINT32          NumBlocks;
+  UINT32          BlockCount;
+
+  // The buffer must be valid
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (Instance->Media.ReadOnly == TRUE) {
+    return EFI_WRITE_PROTECTED;
+  }
+
+  // We must have some bytes to read
+  DEBUG ((DEBUG_BLKIO, "NorFlashWriteBlocks: BufferSizeInBytes=0x%x\n",
+    BufferSizeInBytes));
+  if (BufferSizeInBytes == 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // The size of the buffer must be a multiple of the block size
+  DEBUG ((DEBUG_BLKIO, "NorFlashWriteBlocks: BlockSize in bytes =0x%x\n",
+    Instance->Media.BlockSize));
+  if ((BufferSizeInBytes % Instance->Media.BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // All blocks must be within the device
+  NumBlocks = ((UINT32)BufferSizeInBytes) / Instance->Media.BlockSize ;
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashWriteBlocks: NumBlocks=%d, LastBlock=%ld, Lba=%ld.\n", NumBlocks,
+    Instance->Media.LastBlock, Lba));
+
+  if ((Lba + NumBlocks) > (Instance->Media.LastBlock + 1)) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashWriteBlocks: ERROR - Write will exceed last block.\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  ASSERT (((UINTN)Buffer % sizeof (UINT32)) == 0);
+
+  BlockSizeInWords = Instance->Media.BlockSize / 4;
+
+  // Because the target *Buffer is a pointer to VOID, we must put
+  // all the data into a pointer to a proper data type, so use *ReadBuffer
+  pWriteBuffer = (UINT32 *)Buffer;
+
+  CurrentBlock = Lba;
+  for (BlockCount = 0;
+       BlockCount < NumBlocks;
+       BlockCount++, CurrentBlock++, pWriteBuffer += BlockSizeInWords) {
+
+    DEBUG ((DEBUG_BLKIO, "NorFlashWriteBlocks: Writing block #%d\n",
+      (UINTN)CurrentBlock));
+
+    Status = NorFlashWriteFullBlock (Instance, CurrentBlock, pWriteBuffer,
+               BlockSizeInWords);
+
+    if (EFI_ERROR (Status)) {
+      break;
+    }
+  }
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashWriteBlocks: Exit Status = \"%r\".\n", Status));
+  return Status;
+}
+
+EFI_STATUS
+NorFlashReadBlocks (
+  IN NOR_FLASH_INSTANCE   *Instance,
+  IN EFI_LBA              Lba,
+  IN UINTN                BufferSizeInBytes,
+  OUT VOID                *Buffer
+  )
+{
+  UINT32              NumBlocks;
+  UINTN               StartAddress;
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashReadBlocks: BufferSize=0x%xB BlockSize=0x%xB LastBlock=%ld, Lba=%ld.\n",
+    BufferSizeInBytes, Instance->Media.BlockSize, Instance->Media.LastBlock,
+    Lba));
+
+  // The buffer must be valid
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // Return if we have not any byte to read
+  if (BufferSizeInBytes == 0) {
+    return EFI_SUCCESS;
+  }
+
+  // The size of the buffer must be a multiple of the block size
+  if ((BufferSizeInBytes % Instance->Media.BlockSize) != 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // All blocks must be within the device
+  NumBlocks = ((UINT32)BufferSizeInBytes) / Instance->Media.BlockSize ;
+
+  if ((Lba + NumBlocks) > (Instance->Media.LastBlock + 1)) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashReadBlocks: ERROR - Read will exceed last block\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // Get the address to start reading from
+  StartAddress = GET_NOR_BLOCK_ADDRESS (Instance->RegionBaseAddress, Lba,
+                                        Instance->Media.BlockSize);
+
+  // Put the device into Read Array mode
+  NorFlashSetHostCommand (Instance, SPINOR_OP_READ_4B);
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+
+  // Readout the data
+  CopyMem(Buffer, (UINTN *)StartAddress, BufferSizeInBytes);
+
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+NorFlashRead (
+  IN NOR_FLASH_INSTANCE   *Instance,
+  IN EFI_LBA              Lba,
+  IN UINTN                Offset,
+  IN UINTN                BufferSizeInBytes,
+  OUT VOID                *Buffer
+  )
+{
+  UINTN  StartAddress;
+
+  // The buffer must be valid
+  if (Buffer == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // Return if we have not any byte to read
+  if (BufferSizeInBytes == 0) {
+    return EFI_SUCCESS;
+  }
+
+  if (((Lba * Instance->Media.BlockSize) + Offset + BufferSizeInBytes) >
+      Instance->Size) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashRead: ERROR - Read will exceed device size.\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  // Get the address to start reading from
+  StartAddress = GET_NOR_BLOCK_ADDRESS (Instance->RegionBaseAddress, Lba,
+                                        Instance->Media.BlockSize);
+
+  // Put the device into Read Array mode
+  NorFlashSetHostCommand (Instance, SPINOR_OP_READ_4B);
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+
+  // Readout the data
+  CopyMem (Buffer, (UINTN *)(StartAddress + Offset), BufferSizeInBytes);
+
+  return EFI_SUCCESS;
+}
+
+/*
+  Write a full or portion of a block. It must not span block boundaries;
+  that is, Offset + *NumBytes <= Instance->Media.BlockSize.
+*/
+EFI_STATUS
+NorFlashWriteSingleBlock (
+  IN        NOR_FLASH_INSTANCE   *Instance,
+  IN        EFI_LBA               Lba,
+  IN        UINTN                 Offset,
+  IN OUT    UINTN                *NumBytes,
+  IN        UINT8                *Buffer
+  )
+{
+  EFI_STATUS  TempStatus;
+  UINT32      Tmp;
+  UINT32      TmpBuf;
+  UINT32      WordToWrite;
+  UINT32      Mask;
+  BOOLEAN     DoErase;
+  UINTN       BytesToWrite;
+  UINTN       CurOffset;
+  UINTN       WordAddr;
+  UINTN       BlockSize;
+  UINTN       BlockAddress;
+  UINTN       PrevBlockAddress;
+
+  PrevBlockAddress = 0;
+
+  if (!Instance->Initialized && Instance->Initialize) {
+    Instance->Initialize(Instance);
+  }
+
+  DEBUG ((DEBUG_BLKIO,
+    "NorFlashWriteSingleBlock(Parameters: Lba=%ld, Offset=0x%x, *NumBytes=0x%x, Buffer @ 0x%08x)\n",
+    Lba, Offset, *NumBytes, Buffer));
+
+  // Detect WriteDisabled state
+  if (Instance->Media.ReadOnly == TRUE) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashWriteSingleBlock: ERROR - Can not write: Device is in WriteDisabled state.\n"));
+    // It is in WriteDisabled state, return an error right away
+    return EFI_ACCESS_DENIED;
+  }
+
+  // Cache the block size to avoid de-referencing pointers all the time
+  BlockSize = Instance->Media.BlockSize;
+
+  // The write must not span block boundaries.
+  // We need to check each variable individually because adding two large
+  // values together overflows.
+  if (Offset               >= BlockSize ||
+      *NumBytes            >  BlockSize ||
+      (Offset + *NumBytes) >  BlockSize) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashWriteSingleBlock: ERROR - EFI_BAD_BUFFER_SIZE: (Offset=0x%x + NumBytes=0x%x) > BlockSize=0x%x\n",
+      Offset, *NumBytes, BlockSize ));
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // We must have some bytes to write
+  if (*NumBytes == 0) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashWriteSingleBlock: ERROR - EFI_BAD_BUFFER_SIZE: (Offset=0x%x + NumBytes=0x%x) > BlockSize=0x%x\n",
+      Offset, *NumBytes, BlockSize ));
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // Pick 128bytes as a good start for word operations as opposed to erasing the
+  // block and writing the data regardless if an erase is really needed.
+  // It looks like most individual NV variable writes are smaller than 128bytes.
+  if (*NumBytes <= 128) {
+    // Check to see if we need to erase before programming the data into NOR.
+    // If the destination bits are only changing from 1s to 0s we can just write.
+    // After a block is erased all bits in the block is set to 1.
+    // If any byte requires us to erase we just give up and rewrite all of it.
+    DoErase      = FALSE;
+    BytesToWrite = *NumBytes;
+    CurOffset    = Offset;
+
+    while (BytesToWrite > 0) {
+      // Read full word from NOR, splice as required. A word is the smallest
+      // unit we can write.
+      TempStatus = NorFlashRead (Instance, Lba, CurOffset & ~(0x3), sizeof(Tmp),
+                     &Tmp);
+      if (EFI_ERROR (TempStatus)) {
+        return EFI_DEVICE_ERROR;
+      }
+
+      // Physical address of word in NOR to write.
+      WordAddr = (CurOffset & ~(0x3)) +
+                 GET_NOR_BLOCK_ADDRESS (Instance->RegionBaseAddress, Lba,
+                   BlockSize);
+
+      // The word of data that is to be written.
+      TmpBuf = *((UINT32*)(Buffer + (*NumBytes - BytesToWrite)));
+
+      // First do word aligned chunks.
+      if ((CurOffset & 0x3) == 0) {
+        if (BytesToWrite >= 4) {
+          // Is the destination still in 'erased' state?
+          if (~Tmp != 0) {
+            // Check to see if we are only changing bits to zero.
+            if ((Tmp ^ TmpBuf) & TmpBuf) {
+              DoErase = TRUE;
+              break;
+            }
+          }
+          // Write this word to NOR
+          WordToWrite = TmpBuf;
+          CurOffset += sizeof(TmpBuf);
+          BytesToWrite -= sizeof(TmpBuf);
+        } else {
+          // BytesToWrite < 4. Do small writes and left-overs
+          Mask = ~((~0) << (BytesToWrite * 8));
+          // Mask out the bytes we want.
+          TmpBuf &= Mask;
+          // Is the destination still in 'erased' state?
+          if ((Tmp & Mask) != Mask) {
+            // Check to see if we are only changing bits to zero.
+            if ((Tmp ^ TmpBuf) & TmpBuf) {
+              DoErase = TRUE;
+              break;
+            }
+          }
+          // Merge old and new data. Write merged word to NOR
+          WordToWrite = (Tmp & ~Mask) | TmpBuf;
+          CurOffset += BytesToWrite;
+          BytesToWrite = 0;
+        }
+      } else {
+        // Do multiple words, but starting unaligned.
+        if (BytesToWrite > (4 - (CurOffset & 0x3))) {
+          Mask = ((~0) << ((CurOffset & 0x3) * 8));
+          // Mask out the bytes we want.
+          TmpBuf &= Mask;
+          // Is the destination still in 'erased' state?
+          if ((Tmp & Mask) != Mask) {
+            // Check to see if we are only changing bits to zero.
+            if ((Tmp ^ TmpBuf) & TmpBuf) {
+              DoErase = TRUE;
+              break;
+            }
+          }
+          // Merge old and new data. Write merged word to NOR
+          WordToWrite = (Tmp & ~Mask) | TmpBuf;
+          BytesToWrite -= (4 - (CurOffset & 0x3));
+          CurOffset += (4 - (CurOffset & 0x3));
+        } else {
+          // Unaligned and fits in one word.
+          Mask = (~((~0) << (BytesToWrite * 8))) << ((CurOffset & 0x3) * 8);
+          // Mask out the bytes we want.
+          TmpBuf = (TmpBuf << ((CurOffset & 0x3) * 8)) & Mask;
+          // Is the destination still in 'erased' state?
+          if ((Tmp & Mask) != Mask) {
+            // Check to see if we are only changing bits to zero.
+            if ((Tmp ^ TmpBuf) & TmpBuf) {
+              DoErase = TRUE;
+              break;
+            }
+          }
+          // Merge old and new data. Write merged word to NOR
+          WordToWrite = (Tmp & ~Mask) | TmpBuf;
+          CurOffset += BytesToWrite;
+          BytesToWrite = 0;
+        }
+      }
+
+      //
+      // Write the word to NOR.
+      //
+
+      BlockAddress = GET_NOR_BLOCK_ADDRESS (Instance->RegionBaseAddress, Lba,
+        BlockSize);
+      if (BlockAddress != PrevBlockAddress) {
+        TempStatus = NorFlashUnlockSingleBlockIfNecessary (Instance,
+                       BlockAddress);
+        if (EFI_ERROR (TempStatus)) {
+          return EFI_DEVICE_ERROR;
+        }
+        PrevBlockAddress = BlockAddress;
+      }
+      TempStatus = NorFlashWriteSingleWord (Instance, WordAddr, WordToWrite);
+      if (EFI_ERROR (TempStatus)) {
+        return EFI_DEVICE_ERROR;
+      }
+    }
+    // Exit if we got here and could write all the data. Otherwise do the
+    // Erase-Write cycle.
+    if (!DoErase) {
+      return EFI_SUCCESS;
+    }
+  }
+
+  // Check we did get some memory. Buffer is BlockSize.
+  if (Instance->ShadowBuffer == NULL) {
+    DEBUG ((DEBUG_ERROR, "FvbWrite: ERROR - Buffer not ready\n"));
+    return EFI_DEVICE_ERROR;
+  }
+
+  // Read NOR Flash data into shadow buffer
+  TempStatus = NorFlashReadBlocks (Instance, Lba, BlockSize,
+                 Instance->ShadowBuffer);
+  if (EFI_ERROR (TempStatus)) {
+    // Return one of the pre-approved error statuses
+    return EFI_DEVICE_ERROR;
+  }
+
+  // Put the data at the appropriate location inside the buffer area
+  CopyMem ((VOID*)((UINTN)Instance->ShadowBuffer + Offset), Buffer, *NumBytes);
+
+  // Write the modified buffer back to the NorFlash
+  TempStatus = NorFlashWriteBlocks (Instance, Lba, BlockSize,
+                 Instance->ShadowBuffer);
+  if (EFI_ERROR (TempStatus)) {
+    // Return one of the pre-approved error statuses
+    return EFI_DEVICE_ERROR;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/*
+  Although DiskIoDxe will automatically install the DiskIO protocol whenever
+  we install the BlockIO protocol, its implementation is sub-optimal as it reads
+  and writes entire blocks using the BlockIO protocol. In fact we can access
+  NOR flash with a finer granularity than that, so we can improve performance
+  by directly producing the DiskIO protocol.
+*/
+
+/**
+  Read BufferSize bytes from Offset into Buffer.
+
+  @param  This                  Protocol instance pointer.
+  @param  MediaId               Id of the media, changes every time the media is
+                                replaced.
+  @param  Offset                The starting byte offset to read from
+  @param  BufferSize            Size of Buffer
+  @param  Buffer                Buffer containing read data
+
+  @retval EFI_SUCCESS           The data was read correctly from the device.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the read.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_INVALID_PARAMETER The read request contains device addresses that
+                                are not valid for the device.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+NorFlashDiskIoReadDisk (
+  IN EFI_DISK_IO_PROTOCOL         *This,
+  IN UINT32                       MediaId,
+  IN UINT64                       DiskOffset,
+  IN UINTN                        BufferSize,
+  OUT VOID                        *Buffer
+  )
+{
+  NOR_FLASH_INSTANCE *Instance;
+  UINT32              BlockSize;
+  UINT32              BlockOffset;
+  EFI_LBA             Lba;
+
+  Instance = INSTANCE_FROM_DISKIO_THIS(This);
+
+  if (MediaId != Instance->Media.MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  BlockSize = Instance->Media.BlockSize;
+  Lba = (EFI_LBA) DivU64x32Remainder (DiskOffset, BlockSize, &BlockOffset);
+
+  return NorFlashRead (Instance, Lba, BlockOffset, BufferSize, Buffer);
+}
+
+/**
+  Writes a specified number of bytes to a device.
+
+  @param  This       Indicates a pointer to the calling context.
+  @param  MediaId    ID of the medium to be written.
+  @param  Offset     The starting byte offset on the logical block I/O device to
+                     write.
+  @param  BufferSize The size in bytes of Buffer. The number of bytes to write
+                     to the device.
+  @param  Buffer     A pointer to the buffer containing the data to be written.
+
+  @retval EFI_SUCCESS           The data was written correctly to the device.
+  @retval EFI_WRITE_PROTECTED   The device can not be written to.
+  @retval EFI_DEVICE_ERROR      The device reported an error while performing
+                                the write.
+  @retval EFI_NO_MEDIA          There is no media in the device.
+  @retval EFI_MEDIA_CHNAGED     The MediaId does not matched the current device.
+  @retval EFI_INVALID_PARAMETER The write request contains device addresses that
+                                are not valid for the device.
+
+**/
+STATIC
+EFI_STATUS
+EFIAPI
+NorFlashDiskIoWriteDisk (
+  IN EFI_DISK_IO_PROTOCOL         *This,
+  IN UINT32                       MediaId,
+  IN UINT64                       DiskOffset,
+  IN UINTN                        BufferSize,
+  IN VOID                         *Buffer
+  )
+{
+  NOR_FLASH_INSTANCE *Instance;
+  UINT32              BlockSize;
+  UINT32              BlockOffset;
+  EFI_LBA             Lba;
+  UINTN               RemainingBytes;
+  UINTN               WriteSize;
+  EFI_STATUS          Status;
+
+  Instance = INSTANCE_FROM_DISKIO_THIS(This);
+
+  if (MediaId != Instance->Media.MediaId) {
+    return EFI_MEDIA_CHANGED;
+  }
+
+  BlockSize = Instance->Media.BlockSize;
+  Lba = (EFI_LBA) DivU64x32Remainder (DiskOffset, BlockSize, &BlockOffset);
+
+  RemainingBytes = BufferSize;
+
+  // Write either all the remaining bytes, or the number of bytes that bring
+  // us up to a block boundary, whichever is less.
+  // (DiskOffset | (BlockSize - 1)) + 1) rounds DiskOffset up to the next
+  // block boundary (even if it is already on one).
+  WriteSize = MIN (RemainingBytes,
+                   ((DiskOffset | (BlockSize - 1)) + 1) - DiskOffset);
+
+  do {
+    if (WriteSize == BlockSize) {
+      // Write a full block
+      Status = NorFlashWriteFullBlock (Instance, Lba, Buffer,
+                 BlockSize / sizeof (UINT32));
+    } else {
+      // Write a partial block
+      Status = NorFlashWriteSingleBlock (Instance, Lba, BlockOffset, &WriteSize,
+                 Buffer);
+    }
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
+    // Now continue writing either all the remaining bytes or single blocks.
+    RemainingBytes -= WriteSize;
+    Buffer = (UINT8 *) Buffer + WriteSize;
+    Lba++;
+    BlockOffset = 0;
+    WriteSize = MIN (RemainingBytes, BlockSize);
+  } while (RemainingBytes);
+
+  return Status;
+}
+
+STATIC CONST NOR_FLASH_INSTANCE mNorFlashInstanceTemplate = {
+  NOR_FLASH_SIGNATURE, // Signature
+  NULL, // Handle ... NEED TO BE FILLED
+
+  FALSE, // Initialized
+  NULL, // Initialize
+
+  0, // HostRegisterBaseAddress ... NEED TO BE FILLED
+  0, // DeviceBaseAddress ... NEED TO BE FILLED
+  0, // RegionBaseAddress ... NEED TO BE FILLED
+  0, // Size ... NEED TO BE FILLED
+  0, // StartLba
+  0, // OffsetLba
+
+  {
+    EFI_BLOCK_IO_PROTOCOL_REVISION2,  // Revision
+    NULL,                             // Media ... NEED TO BE FILLED
+    NorFlashBlockIoReset,             // Reset;
+    NorFlashBlockIoReadBlocks,        // ReadBlocks
+    NorFlashBlockIoWriteBlocks,       // WriteBlocks
+    NorFlashBlockIoFlushBlocks        // FlushBlocks
+  }, // BlockIoProtocol
+
+  {
+    0, // MediaId ... NEED TO BE FILLED
+    FALSE, // RemovableMedia
+    TRUE, // MediaPresent
+    FALSE, // LogicalPartition
+    FALSE, // ReadOnly
+    FALSE, // WriteCaching;
+    0, // BlockSize ... NEED TO BE FILLED
+    4, //  IoAlign
+    0, // LastBlock ... NEED TO BE FILLED
+    0, // LowestAlignedLba
+    1, // LogicalBlocksPerPhysicalBlock
+  }, //Media;
+
+  {
+    EFI_DISK_IO_PROTOCOL_REVISION, // Revision
+    NorFlashDiskIoReadDisk,        // ReadDisk
+    NorFlashDiskIoWriteDisk        // WriteDisk
+  },
+  {
+    FvbGetAttributes, // GetAttributes
+    FvbSetAttributes, // SetAttributes
+    FvbGetPhysicalAddress,  // GetPhysicalAddress
+    FvbGetBlockSize,  // GetBlockSize
+    FvbRead,  // Read
+    FvbWrite, // Write
+    FvbEraseBlocks, // EraseBlocks
+    NULL, //ParentHandle
+  }, //  FvbProtoccol;
+
+  NULL, // ShadowBuffer
+  {
+    {
+      {
+        HARDWARE_DEVICE_PATH,
+        HW_VENDOR_DP,
+        {
+          (UINT8)sizeof(VENDOR_DEVICE_PATH),
+          (UINT8)((sizeof(VENDOR_DEVICE_PATH)) >> 8)
+        }
+      },
+      { 0x0, 0x0, 0x0, { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 } },
+    },
+    {
+      END_DEVICE_PATH_TYPE,
+      END_ENTIRE_DEVICE_PATH_SUBTYPE,
+      { sizeof (EFI_DEVICE_PATH_PROTOCOL), 0 }
+    }
+  }, // DevicePath
+
+  NULL, // CmdTable
+  0, // CmdTableSize
+  0 // Flags
+};
+
+STATIC
+EFI_STATUS
+NorFlashCreateInstance (
+  IN UINTN                  HostRegisterBase,
+  IN UINTN                  NorFlashDeviceBase,
+  IN UINTN                  NorFlashRegionBase,
+  IN UINTN                  NorFlashSize,
+  IN UINT32                 MediaId,
+  IN UINT32                 BlockSize,
+  IN BOOLEAN                HasVarStore,
+  IN CONST GUID             *NorFlashGuid,
+  IN CONST CSDC_DEFINITION  *CommandTable,
+  IN UINTN                  CommandTableSize,
+  OUT NOR_FLASH_INSTANCE**  NorFlashInstance
+  )
+{
+  EFI_STATUS Status;
+  NOR_FLASH_INSTANCE* Instance;
+  UINT8 JedecId[3];
+
+  ASSERT(NorFlashInstance != NULL);
+
+  Instance = AllocateRuntimeCopyPool (sizeof mNorFlashInstanceTemplate,
+                                      &mNorFlashInstanceTemplate);
+  if (Instance == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  Instance->HostRegisterBaseAddress = HostRegisterBase;
+  Instance->DeviceBaseAddress       = NorFlashDeviceBase;
+  Instance->RegionBaseAddress       = NorFlashRegionBase;
+  Instance->Size = NorFlashSize;
+
+  Instance->BlockIoProtocol.Media = &Instance->Media;
+  Instance->Media.MediaId = MediaId;
+  Instance->Media.BlockSize = BlockSize;
+  Instance->Media.LastBlock = (NorFlashSize / BlockSize) - 1;
+  Instance->OffsetLba = (NorFlashRegionBase - NorFlashDeviceBase) / BlockSize;
+
+  CopyGuid (&Instance->DevicePath.Vendor.Guid, NorFlashGuid);
+
+  Instance->CmdTable = CommandTable;
+  Instance->CmdTableSize = CommandTableSize;
+  NorFlashReset (Instance);
+
+  NorFlashReadID (Instance, JedecId);
+
+  DEBUG_CODE_BEGIN ();
+    {
+      NOR_FLASH_INFO    *FlashInfo;
+
+      Status = NorFlashGetInfo (JedecId, &FlashInfo, FALSE);
+      if (EFI_ERROR (Status)) {
+        goto FreeInstance;
+      }
+
+      NorFlashPrintInfo (FlashInfo);
+      FreePool (FlashInfo);
+    }
+  DEBUG_CODE_END ();
+
+  Instance->Flags = 0;
+  if (JedecId[0] == NOR_FLASH_ID_STMICRO) {
+    Instance->Flags = NOR_FLASH_POLL_FSR;
+  }
+
+  Instance->ShadowBuffer = AllocateRuntimePool (BlockSize);;
+  if (Instance->ShadowBuffer == NULL) {
+    Status = EFI_OUT_OF_RESOURCES;
+    goto FreeInstance;
+  }
+
+  if (HasVarStore) {
+    Instance->Initialize = NorFlashFvbInitialize;
+  }
+
+  Status = gBS->InstallMultipleProtocolInterfaces (
+                &Instance->Handle,
+                &gEfiDevicePathProtocolGuid, &Instance->DevicePath,
+                &gEfiFirmwareVolumeBlockProtocolGuid, &Instance->FvbProtocol,
+                NULL
+                );
+  if (EFI_ERROR (Status)) {
+    goto FreeInstance;
+  }
+
+  *NorFlashInstance = Instance;
+  return EFI_SUCCESS;
+
+FreeInstance:
+  FreePool (Instance);
+  return Status;
+}
+
+EFI_STATUS
+NorFlashReset (
+  IN  NOR_FLASH_INSTANCE *Instance
+  )
+{
+  FIP006_CS_CFG         CsCfg;
+
+  DEBUG ((DEBUG_BLKIO, "NorFlashReset()\n"));
+  CsCfg.Raw = MmioRead32 (Instance->HostRegisterBaseAddress +
+                          FIP006_REG_CS_CFG); 
+  CsCfg.Reg.MBM = CS_CFG_MBM_SINGLE;
+  CsCfg.Reg.SRAM = CS_CFG_SRAM_RW;
+  MmioWrite32 (Instance->HostRegisterBaseAddress + FIP006_REG_CS_CFG,
+               CsCfg.Raw);
+  NorFlashSetHostCommand (Instance, SPINOR_OP_READ_4B);
+  NorFlashSetHostCSDC (Instance, TRUE, mFip006NullCmdSeq);
+  return EFI_SUCCESS;
+}
+
+EFI_STATUS
+NorFlashReadID (
+  IN  NOR_FLASH_INSTANCE  *Instance,
+  OUT UINT8               JedecId[3]
+  )
+{
+  if (Instance == NULL || JedecId == NULL) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  NorFlashSetHostCommand (Instance, SPINOR_OP_RDID);
+  JedecId[0] = MmioRead8 (Instance->DeviceBaseAddress);
+  JedecId[1] = MmioRead8 (Instance->DeviceBaseAddress + 1);
+  JedecId[2] = MmioRead8 (Instance->DeviceBaseAddress + 2);
+  NorFlashSetHostCommand (Instance, SPINOR_OP_READ_4B);
+  return EFI_SUCCESS;
+}
+
+/**
+  Fixup internal data so that EFI can be call in virtual mode.
+  Call the passed in Child Notify event and convert any pointers in
+  lib to virtual mode.
+
+  @param[in]    Event   The Event that is being processed
+  @param[in]    Context Event Context
+**/
+STATIC
+VOID
+EFIAPI
+NorFlashVirtualNotifyEvent (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  UINTN Index;
+
+  for (Index = 0; Index < mNorFlashDeviceCount; Index++) {
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->HostRegisterBaseAddress);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->DeviceBaseAddress);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->RegionBaseAddress);
+
+    // Convert BlockIo protocol
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->BlockIoProtocol.FlushBlocks);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->BlockIoProtocol.ReadBlocks);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->BlockIoProtocol.Reset);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->BlockIoProtocol.WriteBlocks);
+
+    // Convert Fvb
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.EraseBlocks);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.GetAttributes);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.GetBlockSize);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.GetPhysicalAddress);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.Read);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.SetAttributes);
+    EfiConvertPointer (0x0,
+      (VOID**)&mNorFlashInstances[Index]->FvbProtocol.Write);
+
+    if (mNorFlashInstances[Index]->ShadowBuffer != NULL) {
+      EfiConvertPointer (0x0, (VOID**)&mNorFlashInstances[Index]->ShadowBuffer);
+    }
+
+    EfiConvertPointer (0x0, (VOID**)&mNorFlashInstances[Index]->CmdTable);
+  }
+
+  return;
+}
+
+EFI_STATUS
+EFIAPI
+NorFlashInitialise (
+  IN EFI_HANDLE           ImageHandle,
+  IN EFI_SYSTEM_TABLE     *SystemTable
+  )
+{
+  EFI_STATUS              Status;
+  EFI_PHYSICAL_ADDRESS    HostRegisterBaseAddress;
+  UINT32                  Index;
+  NOR_FLASH_DESCRIPTION*  NorFlashDevices;
+  BOOLEAN                 ContainVariableStorage;
+
+  // Register HSSPI FIP006 register region
+  HostRegisterBaseAddress = PcdGet32 (PcdFip006DxeRegBaseAddress);
+
+  Status = gDS->AddMemorySpace (
+      EfiGcdMemoryTypeMemoryMappedIo,
+      HostRegisterBaseAddress, SIZE_4KB,
+      EFI_MEMORY_UC | EFI_MEMORY_RUNTIME
+      );
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gDS->SetMemorySpaceAttributes (
+      HostRegisterBaseAddress, SIZE_4KB,
+      EFI_MEMORY_UC | EFI_MEMORY_RUNTIME);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = NorFlashPlatformInitialization ();
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR,
+      "NorFlashInitialise: Fail to initialize Nor Flash devices\n"));
+    return Status;
+  }
+
+  // Initialize NOR flash instances
+  Status = NorFlashPlatformGetDevices (&NorFlashDevices, &mNorFlashDeviceCount);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR,"NorFlashInitialise: Fail to get Nor Flash devices\n"));
+    return Status;
+  }
+
+  mNorFlashInstances = AllocateRuntimePool (sizeof(NOR_FLASH_INSTANCE*) *
+                                            mNorFlashDeviceCount);
+
+  for (Index = 0; Index < mNorFlashDeviceCount; Index++) {
+    // Check if this NOR Flash device contain the variable storage region
+    ContainVariableStorage =
+        (NorFlashDevices[Index].RegionBaseAddress <=
+         PcdGet32 (PcdFlashNvStorageVariableBase)) &&
+        (PcdGet32 (PcdFlashNvStorageVariableBase) +
+         PcdGet32 (PcdFlashNvStorageVariableSize) <=
+        NorFlashDevices[Index].RegionBaseAddress + NorFlashDevices[Index].Size);
+
+    Status = NorFlashCreateInstance (
+      HostRegisterBaseAddress,
+      NorFlashDevices[Index].DeviceBaseAddress,
+      NorFlashDevices[Index].RegionBaseAddress,
+      NorFlashDevices[Index].Size,
+      Index,
+      NorFlashDevices[Index].BlockSize,
+      ContainVariableStorage,
+      &NorFlashDevices[Index].Guid,
+      mN25qCSDCDefTable,
+      ARRAY_SIZE (mN25qCSDCDefTable),
+      &mNorFlashInstances[Index]
+    );
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR,
+        "NorFlashInitialise: Fail to create instance for NorFlash[%d]\n",
+        Index));
+    }
+  }
+
+  //
+  // Register for the virtual address change event
+  //
+  Status = gBS->CreateEventEx (EVT_NOTIFY_SIGNAL, TPL_NOTIFY,
+                  NorFlashVirtualNotifyEvent, NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mNorFlashVirtualAddrChangeEvent);
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashDxe.h b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashDxe.h
new file mode 100644
index 000000000000..5e1e6adecf67
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashDxe.h
@@ -0,0 +1,357 @@
+/** @file  NorFlashDxe.h
+
+  Copyright (c) 2011 - 2014, ARM Ltd. All rights reserved.<BR>
+  Copyright (c) 2017, Socionext Inc. All rights reserved.<BR>
+  Copyright (c) 2017, Linaro, Ltd. All rights reserved.<BR>
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#ifndef __NOR_FLASH_DXE_H__
+#define __NOR_FLASH_DXE_H__
+
+
+#include <PiDxe.h>
+
+#include <Guid/EventGroup.h>
+
+#include <Protocol/BlockIo.h>
+#include <Protocol/DiskIo.h>
+#include <Protocol/FirmwareVolumeBlock.h>
+
+#include <Library/DebugLib.h>
+#include <Library/DxeServicesTableLib.h>
+#include <Library/IoLib.h>
+#include <Library/NorFlashPlatformLib.h>
+#include <Library/UefiLib.h>
+#include <Library/UefiRuntimeLib.h>
+
+#include "Fip006Reg.h"
+
+#define NOR_FLASH_ERASE_RETRY                     10
+
+#define GET_NOR_BLOCK_ADDRESS(BaseAddr, Lba, LbaSize) \
+                                      ((BaseAddr) + (UINTN)((Lba) * (LbaSize)))
+
+#define NOR_FLASH_SIGNATURE          SIGNATURE_32('S', 'n', 'o', 'r')
+#define INSTANCE_FROM_FVB_THIS(a)    CR(a, NOR_FLASH_INSTANCE, FvbProtocol, \
+                                        NOR_FLASH_SIGNATURE)
+#define INSTANCE_FROM_BLKIO_THIS(a)  CR(a, NOR_FLASH_INSTANCE, BlockIoProtocol,\
+                                        NOR_FLASH_SIGNATURE)
+#define INSTANCE_FROM_DISKIO_THIS(a) CR(a, NOR_FLASH_INSTANCE, DiskIoProtocol, \
+                                        NOR_FLASH_SIGNATURE)
+
+#define CSDC(Data, Cont, Trp, Dec) \
+  (((Data) << 8) | (((Cont) & 1) << 3) | (((Trp) & 3) << 1) | ((Dec) & 1))
+
+#define CSDC_TRP_MBM              0
+#define CSDC_TRP_DUAL             1
+#define CSDC_TRP_QUAD             2
+#define CSDC_TRP_SINGLE           3
+
+#define CSDC_DEC_LEAVE_ASIS       0
+#define CSDC_DEC_DECODE           1
+
+#define CSDC_CONT_NON_CONTINUOUS  0
+#define CSDC_CONT_CONTINUOUS      1
+
+#define CSDC_ADDRESS_7_0          0x0
+#define CSDC_ADDRESS_15_8         0x1
+#define CSDC_ADDRESS_23_16        0x2
+#define CSDC_ADDRESS_31_24        0x3
+#define CSDC_HIGH_Z               0x4
+#define CSDC_END                  0x7
+
+typedef struct {
+  UINT8       Code;
+  BOOLEAN     AddrAccess;
+  BOOLEAN     AddrMode4Byte;
+  BOOLEAN     HighZ;
+  BOOLEAN     ReadWrite;
+  UINT8       CscfgMbm;
+  UINT8       CsdcTrp;
+} CSDC_DEFINITION;
+
+typedef struct _NOR_FLASH_INSTANCE                NOR_FLASH_INSTANCE;
+
+typedef EFI_STATUS (*NOR_FLASH_INITIALIZE)        (NOR_FLASH_INSTANCE* Instance);
+
+typedef struct {
+  VENDOR_DEVICE_PATH                  Vendor;
+  EFI_DEVICE_PATH_PROTOCOL            End;
+} NOR_FLASH_DEVICE_PATH;
+
+struct _NOR_FLASH_INSTANCE {
+  UINT32                              Signature;
+  EFI_HANDLE                          Handle;
+
+  BOOLEAN                             Initialized;
+  NOR_FLASH_INITIALIZE                Initialize;
+
+  UINTN                               HostRegisterBaseAddress;
+  UINTN                               DeviceBaseAddress;
+  UINTN                               RegionBaseAddress;
+  UINTN                               Size;
+  EFI_LBA                             StartLba;
+  EFI_LBA                             OffsetLba;
+
+  EFI_BLOCK_IO_PROTOCOL               BlockIoProtocol;
+  EFI_BLOCK_IO_MEDIA                  Media;
+  EFI_DISK_IO_PROTOCOL                DiskIoProtocol;
+
+  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL FvbProtocol;
+  VOID*                               ShadowBuffer;
+
+  NOR_FLASH_DEVICE_PATH               DevicePath;
+
+  CONST CSDC_DEFINITION               *CmdTable;
+  UINTN                               CmdTableSize;
+
+  UINT32                              Flags;
+#define NOR_FLASH_POLL_FSR      BIT0
+};
+
+EFI_STATUS
+NorFlashReadCfiData (
+  IN  UINTN                   DeviceBaseAddress,
+  IN  UINTN                   CFI_Offset,
+  IN  UINT32                  NumberOfBytes,
+  OUT UINT32                  *Data
+  );
+
+EFI_STATUS
+NorFlashWriteBuffer (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  TargetAddress,
+  IN UINTN                  BufferSizeInBytes,
+  IN UINT32                 *Buffer
+  );
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.Reset
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoReset (
+  IN EFI_BLOCK_IO_PROTOCOL    *This,
+  IN BOOLEAN                  ExtendedVerification
+  );
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.ReadBlocks
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoReadBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSizeInBytes,
+  OUT VOID                    *Buffer
+);
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.WriteBlocks
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoWriteBlocks (
+  IN  EFI_BLOCK_IO_PROTOCOL   *This,
+  IN  UINT32                  MediaId,
+  IN  EFI_LBA                 Lba,
+  IN  UINTN                   BufferSizeInBytes,
+  IN  VOID                    *Buffer
+);
+
+//
+// BlockIO Protocol function EFI_BLOCK_IO_PROTOCOL.FlushBlocks
+//
+EFI_STATUS
+EFIAPI
+NorFlashBlockIoFlushBlocks (
+  IN EFI_BLOCK_IO_PROTOCOL    *This
+);
+
+//
+// NorFlashFvbDxe.c
+//
+
+EFI_STATUS
+EFIAPI
+NorFlashFvbInitialize (
+  IN NOR_FLASH_INSTANCE*                            Instance
+  );
+
+EFI_STATUS
+EFIAPI
+FvbGetAttributes(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  OUT       EFI_FVB_ATTRIBUTES_2                    *Attributes
+  );
+
+EFI_STATUS
+EFIAPI
+FvbSetAttributes(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  IN OUT    EFI_FVB_ATTRIBUTES_2                    *Attributes
+  );
+
+EFI_STATUS
+EFIAPI
+FvbGetPhysicalAddress(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  OUT       EFI_PHYSICAL_ADDRESS                    *Address
+  );
+
+EFI_STATUS
+EFIAPI
+FvbGetBlockSize(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  IN        EFI_LBA                                 Lba,
+  OUT       UINTN                                   *BlockSize,
+  OUT       UINTN                                   *NumberOfBlocks
+  );
+
+EFI_STATUS
+EFIAPI
+FvbRead(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  IN        EFI_LBA                                 Lba,
+  IN        UINTN                                   Offset,
+  IN OUT    UINTN                                   *NumBytes,
+  IN OUT    UINT8                                   *Buffer
+  );
+
+EFI_STATUS
+EFIAPI
+FvbWrite(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  IN        EFI_LBA                                 Lba,
+  IN        UINTN                                   Offset,
+  IN OUT    UINTN                                   *NumBytes,
+  IN        UINT8                                   *Buffer
+  );
+
+EFI_STATUS
+EFIAPI
+FvbEraseBlocks(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL     *This,
+  ...
+  );
+
+//
+// NorFlashDxe.c
+//
+
+EFI_STATUS
+NorFlashUnlockAndEraseSingleBlock (
+  IN NOR_FLASH_INSTANCE     *Instance,
+  IN UINTN                  BlockAddress
+  );
+
+EFI_STATUS
+NorFlashWriteSingleBlock (
+  IN        NOR_FLASH_INSTANCE   *Instance,
+  IN        EFI_LBA               Lba,
+  IN        UINTN                 Offset,
+  IN OUT    UINTN                *NumBytes,
+  IN        UINT8                *Buffer
+  );
+
+EFI_STATUS
+NorFlashWriteBlocks (
+  IN  NOR_FLASH_INSTANCE *Instance,
+  IN  EFI_LBA           Lba,
+  IN  UINTN             BufferSizeInBytes,
+  IN  VOID              *Buffer
+  );
+
+EFI_STATUS
+NorFlashReadBlocks (
+  IN NOR_FLASH_INSTANCE   *Instance,
+  IN EFI_LBA              Lba,
+  IN UINTN                BufferSizeInBytes,
+  OUT VOID                *Buffer
+  );
+
+EFI_STATUS
+NorFlashRead (
+  IN NOR_FLASH_INSTANCE   *Instance,
+  IN EFI_LBA              Lba,
+  IN UINTN                Offset,
+  IN UINTN                BufferSizeInBytes,
+  OUT VOID                *Buffer
+  );
+
+EFI_STATUS
+NorFlashWrite (
+  IN        NOR_FLASH_INSTANCE   *Instance,
+  IN        EFI_LBA               Lba,
+  IN        UINTN                 Offset,
+  IN OUT    UINTN                *NumBytes,
+  IN        UINT8                *Buffer
+  );
+
+EFI_STATUS
+NorFlashReset (
+  IN  NOR_FLASH_INSTANCE *Instance
+  );
+
+EFI_STATUS
+NorFlashReadID (
+  IN  NOR_FLASH_INSTANCE  *Instance,
+  OUT UINT8               JedecId[3]
+  );
+
+#define SPINOR_SR_WIP                 BIT0  // Write in progress
+#define SPINOR_FSR_READY              BIT7  // Flag Status Register: ready
+
+#define SPINOR_OP_WREN                0x06  // Write enable
+#define SPINOR_OP_WRDIS               0x04  // Write enable
+#define SPINOR_OP_RDSR                0x05  // Read status register
+#define SPINOR_OP_WRSR                0x01  // Write status register 1 byte
+#define SPINOR_OP_RDSR2               0x3f  // Read status register 2
+#define SPINOR_OP_WRSR2               0x3e  // Write status register 2
+#define SPINOR_OP_READ                0x03  // Read data bytes (low frequency)
+#define SPINOR_OP_READ_FAST           0x0b  // Read data bytes (high frequency)
+#define SPINOR_OP_READ_1_1_2          0x3b  // Read data bytes (Dual Output SPI)
+#define SPINOR_OP_READ_1_2_2          0xbb  // Read data bytes (Dual I/O SPI)
+#define SPINOR_OP_READ_1_1_4          0x6b  // Read data bytes (Quad Output SPI)
+#define SPINOR_OP_READ_1_4_4          0xeb  // Read data bytes (Quad I/O SPI)
+#define SPINOR_OP_PP                  0x02  // Page program (up to 256 bytes)
+#define SPINOR_OP_PP_1_1_4            0x32  // Quad page program
+#define SPINOR_OP_PP_1_4_4            0x38  // Quad page program
+#define SPINOR_OP_BE_4K               0x20  // Erase 4KiB block
+#define SPINOR_OP_BE_4K_PMC           0xd7  // Erase 4KiB block on PMC chips
+#define SPINOR_OP_BE_32K              0x52  // Erase 32KiB block
+#define SPINOR_OP_CHIP_ERASE          0xc7  // Erase whole flash chip
+#define SPINOR_OP_SE                  0xd8  // Sector erase (usually 64KiB)
+#define SPINOR_OP_RDID                0x9f  // Read JEDEC ID
+#define SPINOR_OP_RDSFDP              0x5a  // Read SFDP
+#define SPINOR_OP_RDCR                0x35  // Read configuration register
+#define SPINOR_OP_RDFSR               0x70  // Read flag status register
+#define SPINOR_OP_READ_4B             0x13  // Read data bytes (low frequency)
+#define SPINOR_OP_READ_FAST_4B        0x0c  // Read data bytes (high frequency)
+#define SPINOR_OP_READ_1_1_2_4B       0x3c  // Read data bytes (Dual Output SPI)
+#define SPINOR_OP_READ_1_2_2_4B       0xbc  // Read data bytes (Dual I/O SPI)
+#define SPINOR_OP_READ_1_1_4_4B       0x6c  // Read data bytes (Quad Output SPI)
+#define SPINOR_OP_READ_1_4_4_4B       0xec  // Read data bytes (Quad I/O SPI)
+#define SPINOR_OP_PP_4B               0x12  // Page program (up to 256 bytes)
+#define SPINOR_OP_PP_1_1_4_4B         0x34  // Quad page program
+#define SPINOR_OP_PP_1_4_4_4B         0x3e  // Quad page program
+#define SPINOR_OP_BE_4K_4B            0x21  // Erase 4KiB block
+#define SPINOR_OP_BE_32K_4B           0x5c  // Erase 32KiB block
+#define SPINOR_OP_SE_4B               0xdc  // Sector erase (usually 64KiB)
+#define SPINOR_OP_RD_ARRAY            0xe8  // Read array
+#define SPINOR_OP_RD_NVCFG            0xb5  // Read non-volatile config register
+#define SPINOR_OP_RD_VCR              0x85  // Read VCR register
+#define SPINOR_OP_RD_EVCR             0x65  // Read EVCR register
+
+#endif /* __NOR_FLASH_DXE_H__ */
diff --git a/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashFvbDxe.c b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashFvbDxe.c
new file mode 100644
index 000000000000..acc9490b9a5f
--- /dev/null
+++ b/Silicon/Socionext/SynQuacer/Drivers/Fip006Dxe/NorFlashFvbDxe.c
@@ -0,0 +1,853 @@
+/** @file  NorFlashFvbDxe.c
+
+ Copyright (c) 2011 - 2014, ARM Ltd. All rights reserved.<BR>
+ Copyright (c) 2017, Socionext Inc. All rights reserved.<BR>
+ Copyright (c) 2017, Linaro, Ltd. All rights reserved.<BR>
+
+ This program and the accompanying materials are licensed and made available
+ under the terms and conditions of the BSD License which accompanies this
+ distribution.  The full text of the license may be found at
+ http://opensource.org/licenses/bsd-license.php
+
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+ --*/
+
+#include <PiDxe.h>
+
+#include <Guid/VariableFormat.h>
+#include <Guid/SystemNvDataGuid.h>
+
+#include <Library/BaseLib.h>
+#include <Library/BaseMemoryLib.h>
+#include <Library/HobLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/PcdLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiLib.h>
+
+#include "NorFlashDxe.h"
+
+STATIC EFI_EVENT mFvbVirtualAddrChangeEvent;
+STATIC UINTN     mFlashNvStorageVariableBase;
+
+///
+/// The Firmware Volume Block Protocol is the low-level interface
+/// to a firmware volume. File-level access to a firmware volume
+/// should not be done using the Firmware Volume Block Protocol.
+/// Normal access to a firmware volume must use the Firmware
+/// Volume Protocol. Typically, only the file system driver that
+/// produces the Firmware Volume Protocol will bind to the
+/// Firmware Volume Block Protocol.
+///
+
+/**
+  Initialises the FV Header and Variable Store Header
+  to support variable operations.
+
+  @param[in]  Ptr - Location to initialise the headers
+
+**/
+STATIC
+EFI_STATUS
+InitializeFvAndVariableStoreHeaders (
+  IN NOR_FLASH_INSTANCE *Instance
+  )
+{
+  EFI_STATUS                          Status;
+  VOID*                               Headers;
+  UINTN                               HeadersLength;
+  EFI_FIRMWARE_VOLUME_HEADER          *FirmwareVolumeHeader;
+  VARIABLE_STORE_HEADER               *VariableStoreHeader;
+
+  if (!Instance->Initialized && Instance->Initialize) {
+    Instance->Initialize (Instance);
+  }
+
+  HeadersLength = sizeof(EFI_FIRMWARE_VOLUME_HEADER) +
+                  sizeof(EFI_FV_BLOCK_MAP_ENTRY) +
+                  sizeof(VARIABLE_STORE_HEADER);
+  Headers = AllocateZeroPool(HeadersLength);
+
+  //
+  // EFI_FIRMWARE_VOLUME_HEADER
+  //
+  FirmwareVolumeHeader = (EFI_FIRMWARE_VOLUME_HEADER*)Headers;
+  CopyGuid (&FirmwareVolumeHeader->FileSystemGuid, &gEfiSystemNvDataFvGuid);
+  FirmwareVolumeHeader->FvLength =
+      PcdGet32(PcdFlashNvStorageVariableSize) +
+      PcdGet32(PcdFlashNvStorageFtwWorkingSize) +
+      PcdGet32(PcdFlashNvStorageFtwSpareSize);
+  FirmwareVolumeHeader->Signature = EFI_FVH_SIGNATURE;
+  FirmwareVolumeHeader->Attributes = EFI_FVB2_READ_ENABLED_CAP |
+                                     EFI_FVB2_READ_STATUS |
+                                     EFI_FVB2_STICKY_WRITE |
+                                     EFI_FVB2_MEMORY_MAPPED |
+                                     EFI_FVB2_ERASE_POLARITY |
+                                     EFI_FVB2_WRITE_STATUS |
+                                     EFI_FVB2_WRITE_ENABLED_CAP;
+
+  FirmwareVolumeHeader->HeaderLength = sizeof(EFI_FIRMWARE_VOLUME_HEADER) +
+                                       sizeof(EFI_FV_BLOCK_MAP_ENTRY);
+  FirmwareVolumeHeader->Revision = EFI_FVH_REVISION;
+  FirmwareVolumeHeader->BlockMap[0].NumBlocks = Instance->Media.LastBlock + 1;
+  FirmwareVolumeHeader->BlockMap[0].Length      = Instance->Media.BlockSize;
+  FirmwareVolumeHeader->BlockMap[1].NumBlocks = 0;
+  FirmwareVolumeHeader->BlockMap[1].Length      = 0;
+  FirmwareVolumeHeader->Checksum = CalculateCheckSum16 (
+                                     (UINT16*)FirmwareVolumeHeader,
+                                     FirmwareVolumeHeader->HeaderLength);
+
+  //
+  // VARIABLE_STORE_HEADER
+  //
+  VariableStoreHeader = (VOID *)((UINTN)Headers +
+                                 FirmwareVolumeHeader->HeaderLength);
+  CopyGuid (&VariableStoreHeader->Signature, &gEfiAuthenticatedVariableGuid);
+  VariableStoreHeader->Size = PcdGet32(PcdFlashNvStorageVariableSize) -
+                              FirmwareVolumeHeader->HeaderLength;
+  VariableStoreHeader->Format = VARIABLE_STORE_FORMATTED;
+  VariableStoreHeader->State = VARIABLE_STORE_HEALTHY;
+
+  // Install the combined super-header in the NorFlash
+  Status = FvbWrite (&Instance->FvbProtocol, 0, 0, &HeadersLength, Headers);
+
+  FreePool (Headers);
+  return Status;
+}
+
+/**
+  Check the integrity of firmware volume header.
+
+  @param[in] FwVolHeader - A pointer to a firmware volume header
+
+  @retval  EFI_SUCCESS   - The firmware volume is consistent
+  @retval  EFI_NOT_FOUND - The firmware volume has been corrupted.
+
+**/
+EFI_STATUS
+ValidateFvHeader (
+  IN  NOR_FLASH_INSTANCE *Instance
+  )
+{
+  UINT16                      Checksum;
+  EFI_FIRMWARE_VOLUME_HEADER  *FwVolHeader;
+  VARIABLE_STORE_HEADER       *VariableStoreHeader;
+  UINTN                       VariableStoreLength;
+  UINTN                       FvLength;
+
+  FwVolHeader = (EFI_FIRMWARE_VOLUME_HEADER*)Instance->RegionBaseAddress;
+
+  FvLength = PcdGet32(PcdFlashNvStorageVariableSize) +
+             PcdGet32(PcdFlashNvStorageFtwWorkingSize) +
+             PcdGet32(PcdFlashNvStorageFtwSpareSize);
+
+  //
+  // Verify the header revision, header signature, length
+  // Length of FvBlock cannot be 2**64-1
+  // HeaderLength cannot be an odd number
+  //
+  if (   (FwVolHeader->Revision  != EFI_FVH_REVISION)
+      || (FwVolHeader->Signature != EFI_FVH_SIGNATURE)
+      || (FwVolHeader->FvLength  != FvLength)
+      )
+  {
+    DEBUG ((DEBUG_INFO, "%a: No Firmware Volume header present\n",
+      __FUNCTION__));
+    return EFI_NOT_FOUND;
+  }
+
+  // Check the Firmware Volume Guid
+  if (!CompareGuid (&FwVolHeader->FileSystemGuid, &gEfiSystemNvDataFvGuid)) {
+    DEBUG ((DEBUG_INFO, "%a: Firmware Volume Guid non-compatible\n",
+      __FUNCTION__));
+    return EFI_NOT_FOUND;
+  }
+
+  // Verify the header checksum
+  Checksum = CalculateSum16((UINT16*)FwVolHeader, FwVolHeader->HeaderLength);
+  if (Checksum != 0) {
+    DEBUG ((DEBUG_INFO, "%a: FV checksum is invalid (Checksum:0x%X)\n",
+      __FUNCTION__, Checksum));
+    return EFI_NOT_FOUND;
+  }
+
+  VariableStoreHeader = (VOID *)((UINTN)FwVolHeader +
+                                 FwVolHeader->HeaderLength);
+
+  // Check the Variable Store Guid
+  if (!CompareGuid (&VariableStoreHeader->Signature, &gEfiVariableGuid) &&
+      !CompareGuid (&VariableStoreHeader->Signature,
+        &gEfiAuthenticatedVariableGuid)) {
+    DEBUG ((DEBUG_INFO, "%a: Variable Store Guid non-compatible\n",
+      __FUNCTION__));
+    return EFI_NOT_FOUND;
+  }
+
+  VariableStoreLength = PcdGet32 (PcdFlashNvStorageVariableSize) -
+                        FwVolHeader->HeaderLength;
+  if (VariableStoreHeader->Size != VariableStoreLength) {
+    DEBUG ((DEBUG_INFO, "%a: Variable Store Length does not match\n",
+      __FUNCTION__));
+    return EFI_NOT_FOUND;
+  }
+
+  return EFI_SUCCESS;
+}
+
+/**
+ The GetAttributes() function retrieves the attributes and
+ current settings of the block.
+
+ @param This         Indicates the EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL instance.
+
+ @param Attributes   Pointer to EFI_FVB_ATTRIBUTES_2 in which the attributes and
+                     current settings are returned.
+                     Type EFI_FVB_ATTRIBUTES_2 is defined in
+                     EFI_FIRMWARE_VOLUME_HEADER.
+
+ @retval EFI_SUCCESS The firmware volume attributes were returned.
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbGetAttributes(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL    *This,
+  OUT       EFI_FVB_ATTRIBUTES_2                   *Attributes
+  )
+{
+  EFI_FVB_ATTRIBUTES_2  FlashFvbAttributes;
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_FVB_THIS(This);
+
+  FlashFvbAttributes = EFI_FVB2_READ_ENABLED_CAP | EFI_FVB2_READ_STATUS |
+                       EFI_FVB2_STICKY_WRITE | EFI_FVB2_MEMORY_MAPPED |
+                       EFI_FVB2_ERASE_POLARITY;
+
+  // Check if it is write protected
+  if (!Instance->Media.ReadOnly) {
+    FlashFvbAttributes |= EFI_FVB2_WRITE_STATUS | EFI_FVB2_WRITE_ENABLED_CAP;
+  }
+
+  *Attributes = FlashFvbAttributes;
+
+  DEBUG ((DEBUG_BLKIO, "FvbGetAttributes(0x%X)\n", *Attributes));
+
+  return EFI_SUCCESS;
+}
+
+/**
+ The SetAttributes() function sets configurable firmware volume attributes
+ and returns the new settings of the firmware volume.
+
+
+ @param This                     EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL instance.
+
+ @param Attributes               On input, Attributes is a pointer to
+                                 EFI_FVB_ATTRIBUTES_2 that contains the desired
+                                 firmware volume settings.
+                                 On successful return, it contains the new
+                                 settings of the firmware volume.
+
+ @retval EFI_SUCCESS             The firmware volume attributes were returned.
+
+ @retval EFI_INVALID_PARAMETER   The attributes requested are in conflict with
+                                 the capabilities as declared in the firmware
+                                 volume header.
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbSetAttributes(
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL  *This,
+  IN OUT    EFI_FVB_ATTRIBUTES_2                 *Attributes
+  )
+{
+  DEBUG ((DEBUG_BLKIO, "FvbSetAttributes(0x%X) is not supported\n",
+    *Attributes));
+  return EFI_UNSUPPORTED;
+}
+
+/**
+ The GetPhysicalAddress() function retrieves the base address of
+ a memory-mapped firmware volume. This function should be called
+ only for memory-mapped firmware volumes.
+
+ @param This               EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL instance.
+
+ @param Address            Pointer to a caller-allocated
+                           EFI_PHYSICAL_ADDRESS that, on successful
+                           return from GetPhysicalAddress(), contains the
+                           base address of the firmware volume.
+
+ @retval EFI_SUCCESS       The firmware volume base address was returned.
+
+ @retval EFI_NOT_SUPPORTED The firmware volume is not memory mapped.
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbGetPhysicalAddress (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL  *This,
+  OUT       EFI_PHYSICAL_ADDRESS                 *Address
+  )
+{
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_FVB_THIS(This);
+
+  DEBUG ((DEBUG_BLKIO, "FvbGetPhysicalAddress(BaseAddress=0x%08x)\n",
+    Instance->RegionBaseAddress));
+
+  ASSERT(Address != NULL);
+
+  *Address = Instance->RegionBaseAddress;
+  return EFI_SUCCESS;
+}
+
+/**
+ The GetBlockSize() function retrieves the size of the requested
+ block. It also returns the number of additional blocks with
+ the identical size. The GetBlockSize() function is used to
+ retrieve the block map (see EFI_FIRMWARE_VOLUME_HEADER).
+
+
+ @param This                     EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL instance.
+
+ @param Lba                      Indicates the block whose size to return
+
+ @param BlockSize                Pointer to a caller-allocated UINTN in which
+                                 the size of the block is returned.
+
+ @param NumberOfBlocks           Pointer to a caller-allocated UINTN in
+                                 which the number of consecutive blocks,
+                                 starting with Lba, is returned. All
+                                 blocks in this range have a size of
+                                 BlockSize.
+
+
+ @retval EFI_SUCCESS             The firmware volume base address was returned.
+
+ @retval EFI_INVALID_PARAMETER   The requested LBA is out of range.
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbGetBlockSize (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL  *This,
+  IN        EFI_LBA                              Lba,
+  OUT       UINTN                                *BlockSize,
+  OUT       UINTN                                *NumberOfBlocks
+  )
+{
+  EFI_STATUS Status;
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_FVB_THIS(This);
+
+  DEBUG ((DEBUG_BLKIO,
+    "FvbGetBlockSize(Lba=%ld, BlockSize=0x%x, LastBlock=%ld)\n", Lba,
+    Instance->Media.BlockSize, Instance->Media.LastBlock));
+
+  if (Lba > Instance->Media.LastBlock) {
+    DEBUG ((DEBUG_ERROR,
+      "FvbGetBlockSize: ERROR - Parameter LBA %ld is beyond the last Lba (%ld).\n",
+      Lba, Instance->Media.LastBlock));
+    Status = EFI_INVALID_PARAMETER;
+  } else {
+    // This is easy because in this platform each NorFlash device has equal sized blocks.
+    *BlockSize = (UINTN) Instance->Media.BlockSize;
+    *NumberOfBlocks = (UINTN) (Instance->Media.LastBlock - Lba + 1);
+
+    DEBUG ((DEBUG_BLKIO,
+      "FvbGetBlockSize: *BlockSize=0x%x, *NumberOfBlocks=0x%x.\n", *BlockSize,
+      *NumberOfBlocks));
+
+    Status = EFI_SUCCESS;
+  }
+
+  return Status;
+}
+
+/**
+ Reads the specified number of bytes into a buffer from the specified block.
+
+ The Read() function reads the requested number of bytes from the
+ requested block and stores them in the provided buffer.
+ Implementations should be mindful that the firmware volume
+ might be in the ReadDisabled state. If it is in this state,
+ the Read() function must return the status code
+ EFI_ACCESS_DENIED without modifying the contents of the
+ buffer. The Read() function must also prevent spanning block
+ boundaries. If a read is requested that would span a block
+ boundary, the read must read up to the boundary but not
+ beyond. The output parameter NumBytes must be set to correctly
+ indicate the number of bytes actually read. The caller must be
+ aware that a read may be partially completed.
+
+ @param This                 EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL instance.
+
+ @param Lba                  The starting logical block index from which to read
+
+ @param Offset               Offset into the block at which to begin reading.
+
+ @param NumBytes             Pointer to a UINTN.
+                             At entry, *NumBytes contains the total size of the
+                             buffer.
+                             At exit, *NumBytes contains the total number of
+                             bytes read.
+
+ @param Buffer               Pointer to a caller-allocated buffer that will be
+                             used to hold the data that is read.
+
+ @retval EFI_SUCCESS         The firmware volume was read successfully, and
+                             contents are in Buffer.
+
+ @retval EFI_BAD_BUFFER_SIZE Read attempted across an LBA boundary.
+                             On output, NumBytes contains the total number of
+                             bytes returned in Buffer.
+
+ @retval EFI_ACCESS_DENIED   The firmware volume is in the ReadDisabled state.
+
+ @retval EFI_DEVICE_ERROR    The block device is not functioning correctly and
+                             could not be read.
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbRead (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL   *This,
+  IN        EFI_LBA                               Lba,
+  IN        UINTN                                 Offset,
+  IN OUT    UINTN                                 *NumBytes,
+  IN OUT    UINT8                                 *Buffer
+  )
+{
+  EFI_STATUS    TempStatus;
+  UINTN         BlockSize;
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_FVB_THIS(This);
+
+  DEBUG ((DEBUG_BLKIO,
+    "FvbRead(Parameters: Lba=%ld, Offset=0x%x, *NumBytes=0x%x, Buffer @ 0x%08x)\n",
+    Instance->StartLba + Lba, Offset, *NumBytes, Buffer));
+
+  if (!Instance->Initialized && Instance->Initialize) {
+    Instance->Initialize(Instance);
+  }
+
+  TempStatus = EFI_SUCCESS;
+
+  // Cache the block size to avoid de-referencing pointers all the time
+  BlockSize = Instance->Media.BlockSize;
+
+  DEBUG ((DEBUG_BLKIO,
+    "FvbRead: Check if (Offset=0x%x + NumBytes=0x%x) <= BlockSize=0x%x\n",
+    Offset, *NumBytes, BlockSize ));
+
+  // The read must not span block boundaries.
+  // We need to check each variable individually because adding two large
+  // values together overflows.
+  if (Offset               >= BlockSize ||
+      *NumBytes            >  BlockSize ||
+      (Offset + *NumBytes) >  BlockSize) {
+    DEBUG ((DEBUG_ERROR,
+      "FvbRead: ERROR - EFI_BAD_BUFFER_SIZE: (Offset=0x%x + NumBytes=0x%x) > BlockSize=0x%x\n",
+      Offset, *NumBytes, BlockSize ));
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // We must have some bytes to read
+  if (*NumBytes == 0) {
+    return EFI_BAD_BUFFER_SIZE;
+  }
+
+  // Decide if we are doing full block reads or not.
+  if (*NumBytes % BlockSize != 0) {
+    TempStatus = NorFlashRead (Instance, Instance->StartLba + Lba, Offset,
+                   *NumBytes, Buffer);
+    if (EFI_ERROR (TempStatus)) {
+      return EFI_DEVICE_ERROR;
+    }
+  } else {
+    // Read NOR Flash data into shadow buffer
+    TempStatus = NorFlashReadBlocks (Instance, Instance->StartLba + Lba,
+                   BlockSize, Buffer);
+    if (EFI_ERROR (TempStatus)) {
+      // Return one of the pre-approved error statuses
+      return EFI_DEVICE_ERROR;
+    }
+  }
+  return EFI_SUCCESS;
+}
+
+/**
+ Writes the specified number of bytes from the input buffer to the block.
+
+ The Write() function writes the specified number of bytes from
+ the provided buffer to the specified block and offset. If the
+ firmware volume is sticky write, the caller must ensure that
+ all the bits of the specified range to write are in the
+ EFI_FVB_ERASE_POLARITY state before calling the Write()
+ function, or else the result will be unpredictable. This
+ unpredictability arises because, for a sticky-write firmware
+ volume, a write may negate a bit in the EFI_FVB_ERASE_POLARITY
+ state but cannot flip it back again.  Before calling the
+ Write() function,  it is recommended for the caller to first call
+ the EraseBlocks() function to erase the specified block to
+ write. A block erase cycle will transition bits from the
+ (NOT)EFI_FVB_ERASE_POLARITY state back to the
+ EFI_FVB_ERASE_POLARITY state. Implementations should be
+ mindful that the firmware volume might be in the WriteDisabled
+ state. If it is in this state, the Write() function must
+ return the status code EFI_ACCESS_DENIED without modifying the
+ contents of the firmware volume. The Write() function must
+ also prevent spanning block boundaries. If a write is
+ requested that spans a block boundary, the write must store up
+ to the boundary but not beyond. The output parameter NumBytes
+ must be set to correctly indicate the number of bytes actually
+ written. The caller must be aware that a write may be
+ partially completed. All writes, partial or otherwise, must be
+ fully flushed to the hardware before the Write() service
+ returns.
+
+ @param This                 EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL instance.
+
+ @param Lba                  The starting logical block index to write to.
+
+ @param Offset               Offset into the block at which to begin writing.
+
+ @param NumBytes             The pointer to a UINTN.
+                             At entry, *NumBytes contains the total size of the
+                             buffer.
+                             At exit, *NumBytes contains the total number of
+                             bytes actually written.
+
+ @param Buffer               The pointer to a caller-allocated buffer that
+                             contains the source for the write.
+
+ @retval EFI_SUCCESS         The firmware volume was written successfully.
+
+ @retval EFI_BAD_BUFFER_SIZE The write was attempted across an LBA boundary.
+                             On output, NumBytes contains the total number of
+                             bytes actually written.
+
+ @retval EFI_ACCESS_DENIED   The firmware volume is in the WriteDisabled state.
+
+ @retval EFI_DEVICE_ERROR    The block device is malfunctioning and could not be
+                             written.
+
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbWrite (
+  IN CONST  EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL   *This,
+  IN        EFI_LBA                               Lba,
+  IN        UINTN                                 Offset,
+  IN OUT    UINTN                                 *NumBytes,
+  IN        UINT8                                 *Buffer
+  )
+{
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_FVB_THIS (This);
+
+  return NorFlashWriteSingleBlock (Instance, Instance->StartLba + Lba, Offset,
+           NumBytes, Buffer);
+}
+
+/**
+ Erases and initialises a firmware volume block.
+
+ The EraseBlocks() function erases one or more blocks as denoted
+ by the variable argument list. The entire parameter list of
+ blocks must be verified before erasing any blocks. If a block is
+ requested that does not exist within the associated firmware
+ volume (it has a larger index than the last block of the
+ firmware volume), the EraseBlocks() function must return the
+ status code EFI_INVALID_PARAMETER without modifying the contents
+ of the firmware volume. Implementations should be mindful that
+ the firmware volume might be in the WriteDisabled state. If it
+ is in this state, the EraseBlocks() function must return the
+ status code EFI_ACCESS_DENIED without modifying the contents of
+ the firmware volume. All calls to EraseBlocks() must be fully
+ flushed to the hardware before the EraseBlocks() service
+ returns.
+
+ @param This                     EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL
+ instance.
+
+ @param ...                      The variable argument list is a list of tuples.
+                                 Each tuple describes a range of LBAs to erase
+                                 and consists of the following:
+                                 - An EFI_LBA that indicates the starting LBA
+                                 - A UINTN that indicates the number of blocks
+                                   to erase.
+
+                                 The list is terminated with an
+                                 EFI_LBA_LIST_TERMINATOR.
+
+ @retval EFI_SUCCESS             The erase request successfully completed.
+
+ @retval EFI_ACCESS_DENIED       The firmware volume is in the WriteDisabled
+                                 state.
+
+ @retval EFI_DEVICE_ERROR        The block device is not functioning correctly
+                                 and could not be written.
+                                 The firmware device may have been partially
+                                 erased.
+
+ @retval EFI_INVALID_PARAMETER   One or more of the LBAs listed in the variable
+                                 argument list do not exist in the firmware
+                                 volume.
+
+ **/
+EFI_STATUS
+EFIAPI
+FvbEraseBlocks (
+  IN CONST EFI_FIRMWARE_VOLUME_BLOCK2_PROTOCOL *This,
+  ...
+  )
+{
+  EFI_STATUS  Status;
+  VA_LIST     Args;
+  UINTN       BlockAddress; // Physical address of Lba to erase
+  EFI_LBA     StartingLba; // Lba from which we start erasing
+  UINTN       NumOfLba; // Number of Lba blocks to erase
+  NOR_FLASH_INSTANCE *Instance;
+
+  Instance = INSTANCE_FROM_FVB_THIS(This);
+
+  DEBUG ((DEBUG_BLKIO, "FvbEraseBlocks()\n"));
+
+  Status = EFI_SUCCESS;
+
+  // Detect WriteDisabled state
+  if (Instance->Media.ReadOnly) {
+    // Firmware volume is in WriteDisabled state
+    DEBUG ((DEBUG_ERROR,
+      "FvbEraseBlocks: ERROR - Device is in WriteDisabled state.\n"));
+    return EFI_ACCESS_DENIED;
+  }
+
+  // Before erasing, check the entire list of parameters to ensure
+  // all specified blocks are valid
+
+  VA_START (Args, This);
+  do {
+    // Get the Lba from which we start erasing
+    StartingLba = VA_ARG (Args, EFI_LBA);
+
+    // Have we reached the end of the list?
+    if (StartingLba == EFI_LBA_LIST_TERMINATOR) {
+      //Exit the while loop
+      break;
+    }
+
+    // How many Lba blocks are we requested to erase?
+    NumOfLba = VA_ARG (Args, UINT32);
+
+    // All blocks must be within range
+    DEBUG ((DEBUG_BLKIO,
+      "FvbEraseBlocks: Check if: ( StartingLba=%ld + NumOfLba=%d - 1 ) > LastBlock=%ld.\n",
+      Instance->StartLba + StartingLba, NumOfLba, Instance->Media.LastBlock));
+    if (NumOfLba == 0 ||
+        (Instance->StartLba + StartingLba + NumOfLba - 1) >
+        Instance->Media.LastBlock) {
+      VA_END (Args);
+      DEBUG ((DEBUG_ERROR,
+        "FvbEraseBlocks: ERROR - Lba range goes past the last Lba.\n"));
+      Status = EFI_INVALID_PARAMETER;
+      goto EXIT;
+    }
+  } while (TRUE);
+  VA_END (Args);
+
+  //
+  // To get here, all must be ok, so start erasing
+  //
+  VA_START (Args, This);
+  do {
+    // Get the Lba from which we start erasing
+    StartingLba = VA_ARG (Args, EFI_LBA);
+
+    // Have we reached the end of the list?
+    if (StartingLba == EFI_LBA_LIST_TERMINATOR) {
+      // Exit the while loop
+      break;
+    }
+
+    // How many Lba blocks are we requested to erase?
+    NumOfLba = VA_ARG (Args, UINT32);
+
+    // Go through each one and erase it
+    while (NumOfLba > 0) {
+
+      // Get the physical address of Lba to erase
+      BlockAddress = GET_NOR_BLOCK_ADDRESS (Instance->RegionBaseAddress,
+                       Instance->StartLba + StartingLba,
+                       Instance->Media.BlockSize);
+
+      // Erase it
+      DEBUG ((DEBUG_BLKIO, "FvbEraseBlocks: Erasing Lba=%ld @ 0x%08x.\n",
+        Instance->StartLba + StartingLba, BlockAddress));
+      Status = NorFlashUnlockAndEraseSingleBlock (Instance, BlockAddress);
+      if (EFI_ERROR(Status)) {
+        VA_END (Args);
+        Status = EFI_DEVICE_ERROR;
+        goto EXIT;
+      }
+
+      // Move to the next Lba
+      StartingLba++;
+      NumOfLba--;
+    }
+  } while (TRUE);
+  VA_END (Args);
+
+EXIT:
+  return Status;
+}
+
+/**
+  Fixup internal data so that EFI can be call in virtual mode.
+  Call the passed in Child Notify event and convert any pointers in
+  lib to virtual mode.
+
+  @param[in]    Event   The Event that is being processed
+  @param[in]    Context Event Context
+**/
+STATIC
+VOID
+EFIAPI
+FvbVirtualNotifyEvent (
+  IN EFI_EVENT        Event,
+  IN VOID             *Context
+  )
+{
+  EfiConvertPointer (0x0, (VOID**)&mFlashNvStorageVariableBase);
+  return;
+}
+
+EFI_STATUS
+EFIAPI
+NorFlashFvbInitialize (
+  IN NOR_FLASH_INSTANCE* Instance
+  )
+{
+  EFI_STATUS      Status;
+  UINT32          FvbNumLba;
+  EFI_BOOT_MODE   BootMode;
+  UINTN           RuntimeMmioRegionSize;
+  UINTN           BlockSize;
+
+  DEBUG ((DEBUG_BLKIO,"NorFlashFvbInitialize\n"));
+
+  BlockSize = Instance->Media.BlockSize;
+
+  // FirmwareVolumeHeader->FvLength is declared to have the Variable area
+  // AND the FTW working area AND the FTW Spare contiguous.
+  ASSERT(PcdGet32(PcdFlashNvStorageVariableBase) +
+         PcdGet32(PcdFlashNvStorageVariableSize) ==
+         PcdGet32(PcdFlashNvStorageFtwWorkingBase));
+  ASSERT(PcdGet32(PcdFlashNvStorageFtwWorkingBase) +
+         PcdGet32(PcdFlashNvStorageFtwWorkingSize) ==
+         PcdGet32(PcdFlashNvStorageFtwSpareBase));
+
+  // Check if the size of the area is at least one block size
+  ASSERT((PcdGet32(PcdFlashNvStorageVariableSize) > 0) &&
+         (PcdGet32(PcdFlashNvStorageVariableSize) / BlockSize > 0));
+  ASSERT((PcdGet32(PcdFlashNvStorageFtwWorkingSize) > 0) &&
+         (PcdGet32(PcdFlashNvStorageFtwWorkingSize) / BlockSize > 0));
+  ASSERT((PcdGet32(PcdFlashNvStorageFtwSpareSize) > 0) &&
+         (PcdGet32(PcdFlashNvStorageFtwSpareSize) / BlockSize > 0));
+
+  // Ensure the Variable areas are aligned on block size boundaries
+  ASSERT((PcdGet32(PcdFlashNvStorageVariableBase) % BlockSize) == 0);
+  ASSERT((PcdGet32(PcdFlashNvStorageFtwWorkingBase) % BlockSize) == 0);
+  ASSERT((PcdGet32(PcdFlashNvStorageFtwSpareBase) % BlockSize) == 0);
+
+
+  Instance->Initialized = TRUE;
+  mFlashNvStorageVariableBase = FixedPcdGet32 (PcdFlashNvStorageVariableBase);
+
+  // Set the index of the first LBA for the FVB
+  Instance->StartLba = (PcdGet32 (PcdFlashNvStorageVariableBase) -
+                        Instance->RegionBaseAddress) / BlockSize;
+
+  BootMode = GetBootModeHob ();
+  if (BootMode == BOOT_WITH_DEFAULT_SETTINGS) {
+    Status = EFI_INVALID_PARAMETER;
+  } else {
+    // Determine if there is a valid header at the beginning of the NorFlash
+    Status = ValidateFvHeader (Instance);
+  }
+
+  // Install the Default FVB header if required
+  if (EFI_ERROR(Status)) {
+    // There is no valid header, so time to install one.
+    DEBUG ((DEBUG_INFO, "%a: The FVB Header is not valid.\n", __FUNCTION__));
+    DEBUG ((DEBUG_INFO, "%a: Installing a correct one for this volume.\n",
+      __FUNCTION__));
+
+    // Erase all the NorFlash that is reserved for variable storage
+    FvbNumLba = (PcdGet32(PcdFlashNvStorageVariableSize) +
+                 PcdGet32(PcdFlashNvStorageFtwWorkingSize) +
+                 PcdGet32(PcdFlashNvStorageFtwSpareSize)) /
+                Instance->Media.BlockSize;
+
+    Status = FvbEraseBlocks (&Instance->FvbProtocol, (EFI_LBA)0, FvbNumLba,
+               EFI_LBA_LIST_TERMINATOR);
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+
+    // Install all appropriate headers
+    Status = InitializeFvAndVariableStoreHeaders (Instance);
+    if (EFI_ERROR(Status)) {
+      return Status;
+    }
+  }
+
+  //
+  // Declare the Non-Volatile storage as EFI_MEMORY_RUNTIME
+  //
+  RuntimeMmioRegionSize = Instance->Size;
+
+  Status = gDS->AddMemorySpace (EfiGcdMemoryTypeMemoryMappedIo,
+                  Instance->RegionBaseAddress, RuntimeMmioRegionSize,
+                  EFI_MEMORY_UC | EFI_MEMORY_RUNTIME);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gDS->AddMemorySpace (EfiGcdMemoryTypeMemoryMappedIo,
+                  Instance->DeviceBaseAddress, SIZE_4KB,
+                  EFI_MEMORY_UC | EFI_MEMORY_RUNTIME);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gDS->SetMemorySpaceAttributes (Instance->RegionBaseAddress,
+                  RuntimeMmioRegionSize, EFI_MEMORY_UC | EFI_MEMORY_RUNTIME);
+  ASSERT_EFI_ERROR (Status);
+
+  Status = gDS->SetMemorySpaceAttributes (Instance->DeviceBaseAddress,
+                  SIZE_4KB, EFI_MEMORY_UC | EFI_MEMORY_RUNTIME);
+  ASSERT_EFI_ERROR (Status);
+
+  //
+  // Register for the virtual address change event
+  //
+  Status = gBS->CreateEventEx (
+                  EVT_NOTIFY_SIGNAL,
+                  TPL_NOTIFY,
+                  FvbVirtualNotifyEvent,
+                  NULL,
+                  &gEfiEventVirtualAddressChangeGuid,
+                  &mFvbVirtualAddrChangeEvent
+                  );
+  ASSERT_EFI_ERROR (Status);
+
+  return Status;
+}
