[platforms: PATCH v2 12/12] Marvell/Armada7k8k: Introduce NonDiscoverable device init routines

[Marcin Wojtas <mw@semihalf.com>]

To abstract the initialization required for non-discoverable devices,
which is often platform specific (e.g. enable GPIO-driven power supply),
introduce a NonDiscoverableInitLib for use by the NonDiscoverable code,
for which each platform can supply its own version.

Add XHCI power supply (so called 'VBUS') enabling routines
for supported platforms (Armada70x0Db, Armada80x0Db, Armada80x0McBin).

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Marcin Wojtas <mw@semihalf.com>
---
 Silicon/Marvell/Marvell.dec                                                         |   1 +
 Platform/Marvell/Armada70x0Db/Armada70x0Db.dsc                                      |   3 +
 Platform/Marvell/Armada80x0Db/Armada80x0Db.dsc                                      |   3 +
 Platform/SolidRun/Armada80x0McBin/Armada80x0McBin.dsc                               |   3 +
 Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf     |  48 +++++++++
 Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf     |  49 +++++++++
 Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.inf |  49 +++++++++
 Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.inf                   |   1 +
 Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h       |  23 +++++
 Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h       |  28 ++++++
 Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.h   |  20 ++++
 Silicon/Marvell/Include/Library/NonDiscoverableInitLib.h                            |  28 ++++++
 Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c       |  96 ++++++++++++++++++
 Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c       | 106 ++++++++++++++++++++
 Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.c   |  71 +++++++++++++
 Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.c                     |  13 ++-
 16 files changed, 539 insertions(+), 3 deletions(-)
 create mode 100644 Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
 create mode 100644 Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
 create mode 100644 Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
 create mode 100644 Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h
 create mode 100644 Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h
 create mode 100644 Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.h
 create mode 100644 Silicon/Marvell/Include/Library/NonDiscoverableInitLib.h
 create mode 100644 Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c
 create mode 100644 Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c
 create mode 100644 Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.c

diff --git a/Silicon/Marvell/Marvell.dec b/Silicon/Marvell/Marvell.dec
index 616624e..12bac55 100644
--- a/Silicon/Marvell/Marvell.dec
+++ b/Silicon/Marvell/Marvell.dec
@@ -63,6 +63,7 @@
   ArmadaBoardDescLib|Include/Library/ArmadaBoardDescLib.h
   ArmadaIcuLib|Include/Library/ArmadaIcuLib.h
   ArmadaSoCDescLib|Include/Library/ArmadaSoCDescLib.h
+  NonDiscoverableInitLib|Include/Library/NonDiscoverableInitLib.h
   SampleAtResetLib|Include/Library/SampleAtResetLib.h
 
 [Protocols]
diff --git a/Platform/Marvell/Armada70x0Db/Armada70x0Db.dsc b/Platform/Marvell/Armada70x0Db/Armada70x0Db.dsc
index 31815e4..e8cd177 100644
--- a/Platform/Marvell/Armada70x0Db/Armada70x0Db.dsc
+++ b/Platform/Marvell/Armada70x0Db/Armada70x0Db.dsc
@@ -48,6 +48,9 @@
 
 !include Silicon/Marvell/Armada7k8k/Armada7k8k.dsc.inc
 
+[LibraryClasses.common]
+  NonDiscoverableInitLib|Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
+
 [Components.common]
   Silicon/Marvell/Armada7k8k/DeviceTree/Armada70x0Db.inf
 
diff --git a/Platform/Marvell/Armada80x0Db/Armada80x0Db.dsc b/Platform/Marvell/Armada80x0Db/Armada80x0Db.dsc
index 42f7bd3..8e8c2ba 100644
--- a/Platform/Marvell/Armada80x0Db/Armada80x0Db.dsc
+++ b/Platform/Marvell/Armada80x0Db/Armada80x0Db.dsc
@@ -48,6 +48,9 @@
 
 !include Silicon/Marvell/Armada7k8k/Armada7k8k.dsc.inc
 
+[LibraryClasses.common]
+  NonDiscoverableInitLib|Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
+
 [Components.common]
   Silicon/Marvell/Armada7k8k/DeviceTree/Armada80x0Db.inf
 
diff --git a/Platform/SolidRun/Armada80x0McBin/Armada80x0McBin.dsc b/Platform/SolidRun/Armada80x0McBin/Armada80x0McBin.dsc
index 077224d..d080136 100644
--- a/Platform/SolidRun/Armada80x0McBin/Armada80x0McBin.dsc
+++ b/Platform/SolidRun/Armada80x0McBin/Armada80x0McBin.dsc
@@ -49,6 +49,9 @@
 
 !include Silicon/Marvell/Armada7k8k/Armada7k8k.dsc.inc
 
+[LibraryClasses.common]
+  NonDiscoverableInitLib|Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
+
 [Components.common]
   Silicon/Marvell/Armada7k8k/DeviceTree/Armada80x0McBin.inf
 
diff --git a/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf b/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
new file mode 100644
index 0000000..8d33e1d
--- /dev/null
+++ b/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
@@ -0,0 +1,48 @@
+## @file
+#
+#  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                      = Armada70x0DbNonDiscoverableInitLib
+  FILE_GUID                      = 151b04bc-9195-4380-b1fa-987130b450f0
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NonDiscoverableInitLib
+
+#
+# The following information is for reference only and not required by the build
+# tools.
+#
+#  VALID_ARCHITECTURES           = AARCH64
+#
+
+[Sources]
+  NonDiscoverableInitLib.c
+
+[Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  Silicon/Marvell/Marvell.dec
+
+[LibraryClasses]
+  DebugLib
+  IoLib
+
+[Protocols]
+  gEmbeddedGpioProtocolGuid
+
+[Depex]
+  gMarvellPlatformInitCompleteProtocolGuid
diff --git a/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf b/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
new file mode 100644
index 0000000..e69e48a
--- /dev/null
+++ b/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
@@ -0,0 +1,49 @@
+## @file
+#
+#  Copyright (c) 2017, Linaro Ltd. All rights reserved.<BR>
+#  Copyright (c) 2018, Marvell International 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                      = Armada80x0DbNonDiscoverableInitLib
+  FILE_GUID                      = 719c91d5-25aa-4366-988a-1642ae3a6734
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NonDiscoverableInitLib
+
+#
+# The following information is for reference only and not required by the build
+# tools.
+#
+#  VALID_ARCHITECTURES           = AARCH64
+#
+
+[Sources]
+  NonDiscoverableInitLib.c
+
+[Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  Silicon/Marvell/Marvell.dec
+
+[LibraryClasses]
+  DebugLib
+  IoLib
+
+[Protocols]
+  gEmbeddedGpioProtocolGuid
+
+[Depex]
+  gMarvellPlatformInitCompleteProtocolGuid
diff --git a/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.inf b/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
new file mode 100644
index 0000000..d0190b6
--- /dev/null
+++ b/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.inf
@@ -0,0 +1,49 @@
+## @file
+#
+#  Copyright (c) 2017, Linaro Ltd. All rights reserved.<BR>
+#  Copyright (c) 2018, Marvell International 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                      = Armada80x0McBinNonDiscoverableInitLib
+  FILE_GUID                      = 470963c4-476e-4b85-a7c4-17868177f441
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = NonDiscoverableInitLib
+
+#
+# The following information is for reference only and not required by the build
+# tools.
+#
+#  VALID_ARCHITECTURES           = AARCH64
+#
+
+[Sources]
+  NonDiscoverableInitLib.c
+
+[Packages]
+  EmbeddedPkg/EmbeddedPkg.dec
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  Silicon/Marvell/Marvell.dec
+
+[LibraryClasses]
+  DebugLib
+  IoLib
+
+[Protocols]
+  gEmbeddedGpioProtocolGuid
+
+[Depex]
+  gMarvellPlatformInitCompleteProtocolGuid
diff --git a/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.inf b/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.inf
index 98e5b0c..3f9f7bc 100644
--- a/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.inf
+++ b/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.inf
@@ -50,6 +50,7 @@
 
 [LibraryClasses]
   NonDiscoverableDeviceRegistrationLib
+  NonDiscoverableInitLib
   UefiDriverEntryPoint
 
 [Protocols]
diff --git a/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h b/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h
new file mode 100644
index 0000000..8a0e424
--- /dev/null
+++ b/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h
@@ -0,0 +1,23 @@
+/**
+*
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 __NON_DISCOVERABLE_INIT_LIB_H__
+#define __NON_DISCOVERABLE_INIT_LIB_H__
+
+#define ARMADA_70x0_DB_IO_EXPANDER0       0
+#define ARMADA_70x0_DB_VBUS0_PIN          0
+#define ARMADA_70x0_DB_VBUS0_LIMIT_PIN    4
+#define ARMADA_70x0_DB_VBUS1_PIN          1
+#define ARMADA_70x0_DB_VBUS1_LIMIT_PIN    5
+
+#endif
diff --git a/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h b/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h
new file mode 100644
index 0000000..64373ff
--- /dev/null
+++ b/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.h
@@ -0,0 +1,28 @@
+/**
+*
+*  Copyright (c) 2017, Linaro Ltd. All rights reserved.
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 __NON_DISCOVERABLE_INIT_LIB_H__
+#define __NON_DISCOVERABLE_INIT_LIB_H__
+
+#define ARMADA_80x0_DB_IO_EXPANDER0       0
+#define ARMADA_80x0_DB_VBUS0_PIN          0
+#define ARMADA_80x0_DB_VBUS0_LIMIT_PIN    4
+#define ARMADA_80x0_DB_VBUS1_PIN          1
+#define ARMADA_80x0_DB_VBUS1_LIMIT_PIN    5
+
+#define ARMADA_80x0_DB_IO_EXPANDER1       1
+#define ARMADA_80x0_DB_VBUS2_PIN          0
+#define ARMADA_80x0_DB_VBUS2_LIMIT_PIN    4
+
+#endif
diff --git a/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.h b/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.h
new file mode 100644
index 0000000..38e1f20
--- /dev/null
+++ b/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.h
@@ -0,0 +1,20 @@
+/**
+*
+*  Copyright (c) 2017, Linaro Ltd. All rights reserved.
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 __NON_DISCOVERABLE_INIT_LIB_H__
+#define __NON_DISCOVERABLE_INIT_LIB_H__
+
+#define ARMADA_80x0_MCBIN_VBUS0_PIN               15
+
+#endif
diff --git a/Silicon/Marvell/Include/Library/NonDiscoverableInitLib.h b/Silicon/Marvell/Include/Library/NonDiscoverableInitLib.h
new file mode 100644
index 0000000..18de24f
--- /dev/null
+++ b/Silicon/Marvell/Include/Library/NonDiscoverableInitLib.h
@@ -0,0 +1,28 @@
+/**
+*
+*  Copyright (c) 2017, Linaro Ltd. All rights reserved.
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 __NON_DISCOVERABLE_INIT_LIB_H__
+#define __NON_DISCOVERABLE_INIT_LIB_H__
+
+#include <Library/NonDiscoverableDeviceRegistrationLib.h>
+
+NON_DISCOVERABLE_DEVICE_INIT
+EFIAPI
+NonDiscoverableDeviceInitializerGet (
+  IN  NON_DISCOVERABLE_DEVICE_TYPE  Type,
+  IN  UINTN                         Index
+  );
+
+#endif
diff --git a/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c b/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c
new file mode 100644
index 0000000..586b189
--- /dev/null
+++ b/Platform/Marvell/Armada70x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c
@@ -0,0 +1,96 @@
+/**
+*
+*  Copyright (c) 2017, Linaro Ltd. All rights reserved.
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 <Uefi.h>
+
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/IoLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/NonDiscoverableDeviceRegistrationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include <Protocol/MvGpio.h>
+#include <Protocol/NonDiscoverableDevice.h>
+
+#include "NonDiscoverableInitLib.h"
+
+STATIC CONST MV_GPIO_PIN mXhciVbusPins[] = {
+  {
+    ARMADA_70x0_DB_IO_EXPANDER0,
+    ARMADA_70x0_DB_VBUS0_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_70x0_DB_IO_EXPANDER0,
+    ARMADA_70x0_DB_VBUS0_LIMIT_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_70x0_DB_IO_EXPANDER0,
+    ARMADA_70x0_DB_VBUS1_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_70x0_DB_IO_EXPANDER0,
+    ARMADA_70x0_DB_VBUS1_LIMIT_PIN,
+    TRUE,
+  },
+};
+
+STATIC
+EFI_STATUS
+EFIAPI
+XhciInit (
+  IN  NON_DISCOVERABLE_DEVICE  *This
+  )
+{
+  CONST MV_GPIO_PIN   *VbusPin;
+  EMBEDDED_GPIO_MODE   Mode;
+  EMBEDDED_GPIO_PIN    Gpio;
+  EMBEDDED_GPIO       *GpioProtocol;
+  EFI_STATUS           Status;
+  UINTN                Index;
+
+  Status = MvGpioGetProtocol (MV_GPIO_DRIVER_TYPE_PCA95XX, &GpioProtocol);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: Unable to find GPIO protocol\n", __FUNCTION__));
+    return Status;
+  }
+
+  VbusPin = mXhciVbusPins;
+  for (Index = 0; Index < ARRAY_SIZE (mXhciVbusPins); Index++) {
+    Mode = VbusPin->ActiveHigh ? GPIO_MODE_OUTPUT_1 : GPIO_MODE_OUTPUT_0;
+    Gpio = GPIO (VbusPin->ControllerId, VbusPin->PinNumber);
+    GpioProtocol->Set (GpioProtocol, Gpio, Mode);
+    VbusPin++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+NON_DISCOVERABLE_DEVICE_INIT
+EFIAPI
+NonDiscoverableDeviceInitializerGet (
+  IN  NON_DISCOVERABLE_DEVICE_TYPE  Type,
+  IN  UINTN                         Index
+  )
+{
+  if (Type == NonDiscoverableDeviceTypeXhci) {
+        return XhciInit;
+  }
+
+  return NULL;
+}
diff --git a/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c b/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c
new file mode 100644
index 0000000..3988f9a
--- /dev/null
+++ b/Platform/Marvell/Armada80x0Db/NonDiscoverableInitLib/NonDiscoverableInitLib.c
@@ -0,0 +1,106 @@
+/**
+*
+*  Copyright (c) 2017, Linaro Ltd. All rights reserved.
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 <Uefi.h>
+
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/IoLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/NonDiscoverableDeviceRegistrationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include <Protocol/MvGpio.h>
+#include <Protocol/NonDiscoverableDevice.h>
+
+#include "NonDiscoverableInitLib.h"
+
+STATIC CONST MV_GPIO_PIN mXhciVbusPins[] = {
+  {
+    ARMADA_80x0_DB_IO_EXPANDER0,
+    ARMADA_80x0_DB_VBUS0_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_80x0_DB_IO_EXPANDER0,
+    ARMADA_80x0_DB_VBUS0_LIMIT_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_80x0_DB_IO_EXPANDER0,
+    ARMADA_80x0_DB_VBUS1_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_80x0_DB_IO_EXPANDER0,
+    ARMADA_80x0_DB_VBUS1_LIMIT_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_80x0_DB_IO_EXPANDER1,
+    ARMADA_80x0_DB_VBUS2_PIN,
+    TRUE,
+  },
+  {
+    ARMADA_80x0_DB_IO_EXPANDER1,
+    ARMADA_80x0_DB_VBUS2_LIMIT_PIN,
+    TRUE,
+  },
+};
+
+STATIC
+EFI_STATUS
+EFIAPI
+XhciInit (
+  IN  NON_DISCOVERABLE_DEVICE  *This
+  )
+{
+  CONST MV_GPIO_PIN   *VbusPin;
+  EMBEDDED_GPIO_MODE   Mode;
+  EMBEDDED_GPIO_PIN    Gpio;
+  EMBEDDED_GPIO       *GpioProtocol;
+  EFI_STATUS           Status;
+  UINTN                Index;
+
+  Status = MvGpioGetProtocol (MV_GPIO_DRIVER_TYPE_PCA95XX, &GpioProtocol);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: Unable to find GPIO protocol\n", __FUNCTION__));
+    return Status;
+  }
+
+  VbusPin = mXhciVbusPins;
+  for (Index = 0; Index < ARRAY_SIZE (mXhciVbusPins); Index++) {
+    Mode = VbusPin->ActiveHigh ? GPIO_MODE_OUTPUT_1 : GPIO_MODE_OUTPUT_0;
+    Gpio = GPIO (VbusPin->ControllerId, VbusPin->PinNumber);
+    GpioProtocol->Set (GpioProtocol, Gpio, Mode);
+    VbusPin++;
+  }
+
+  return EFI_SUCCESS;
+}
+
+NON_DISCOVERABLE_DEVICE_INIT
+EFIAPI
+NonDiscoverableDeviceInitializerGet (
+  IN  NON_DISCOVERABLE_DEVICE_TYPE  Type,
+  IN  UINTN                         Index
+  )
+{
+  if (Type == NonDiscoverableDeviceTypeXhci) {
+        return XhciInit;
+  }
+
+  return NULL;
+}
diff --git a/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.c b/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.c
new file mode 100644
index 0000000..676feeb
--- /dev/null
+++ b/Platform/SolidRun/Armada80x0McBin/NonDiscoverableInitLib/NonDiscoverableInitLib.c
@@ -0,0 +1,71 @@
+/**
+*
+*  Copyright (c) 2017, Linaro Ltd. All rights reserved.
+*  Copyright (c) 2018, Marvell International Ltd. All rights reserved.
+*
+*  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 <Uefi.h>
+
+#include <Library/DebugLib.h>
+#include <Library/DevicePathLib.h>
+#include <Library/IoLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/NonDiscoverableDeviceRegistrationLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+
+#include <Protocol/MvGpio.h>
+#include <Protocol/NonDiscoverableDevice.h>
+
+#include "NonDiscoverableInitLib.h"
+
+STATIC CONST MV_GPIO_PIN mXhciVbusPin = {
+  MV_GPIO_CP0_CONTROLLER1,
+  ARMADA_80x0_MCBIN_VBUS0_PIN,
+  TRUE,
+};
+
+STATIC
+EFI_STATUS
+EFIAPI
+XhciInit (
+  IN  NON_DISCOVERABLE_DEVICE  *This
+  )
+{
+  EMBEDDED_GPIO  *GpioProtocol;
+  EFI_STATUS      Status;
+
+  Status = MvGpioGetProtocol (MV_GPIO_DRIVER_TYPE_SOC_CONTROLLER, &GpioProtocol);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "%a: Unable to find GPIO protocol\n", __FUNCTION__));
+    return Status;
+  }
+
+  GpioProtocol->Set (GpioProtocol,
+                  GPIO (mXhciVbusPin.ControllerId, mXhciVbusPin.PinNumber),
+                  GPIO_MODE_OUTPUT_1);
+
+  return Status;
+}
+
+NON_DISCOVERABLE_DEVICE_INIT
+EFIAPI
+NonDiscoverableDeviceInitializerGet (
+  IN  NON_DISCOVERABLE_DEVICE_TYPE  Type,
+  IN  UINTN                         Index
+  )
+{
+  if (Type == NonDiscoverableDeviceTypeXhci) {
+        return XhciInit;
+  }
+
+  return NULL;
+}
diff --git a/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.c b/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.c
index 116e9d4..74040b2 100644
--- a/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.c
+++ b/Silicon/Marvell/Drivers/NonDiscoverableDxe/NonDiscoverableDxe.c
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <Library/DebugLib.h>
 #include <Library/NonDiscoverableDeviceRegistrationLib.h>
+#include <Library/NonDiscoverableInitLib.h>
 #include <Library/UefiBootServicesTableLib.h>
 
 #include <Protocol/BoardDesc.h>
@@ -57,7 +58,9 @@ NonDiscoverableInitXhci (
     Status = RegisterNonDiscoverableMmioDevice (
                      NonDiscoverableDeviceTypeXhci,
                      Desc[i].SoC->XhciDmaType,
-                     NULL,
+                     NonDiscoverableDeviceInitializerGet (
+                       NonDiscoverableDeviceTypeXhci,
+                       i),
                      NULL,
                      1,
                      Desc[i].SoC->XhciBaseAddress,
@@ -86,7 +89,9 @@ NonDiscoverableInitAhci (
     Status = RegisterNonDiscoverableMmioDevice (
                      NonDiscoverableDeviceTypeAhci,
                      Desc[i].SoC->AhciDmaType,
-                     NULL,
+                     NonDiscoverableDeviceInitializerGet (
+                       NonDiscoverableDeviceTypeAhci,
+                       i),
                      NULL,
                      1,
                      Desc[i].SoC->AhciBaseAddress,
@@ -115,7 +120,9 @@ NonDiscoverableInitSdhci (
     Status = RegisterNonDiscoverableMmioDevice (
                      NonDiscoverableDeviceTypeSdhci,
                      Desc[i].SoC->SdMmcDmaType,
-                     NULL,
+                     NonDiscoverableDeviceInitializerGet (
+                       NonDiscoverableDeviceTypeSdhci,
+                       i),
                      NULL,
                      1,
                      Desc[i].SoC->SdMmcBaseAddress,
-- 
2.7.4