12 files changed, 4162 insertions, 0 deletions
diff --git a/Platform/Marvell/Library/ComPhyLib/ComPhyCp110.c b/Platform/Marvell/Library/ComPhyLib/ComPhyCp110.c
new file mode 100755
index 0000000000..40a7b9921c
--- /dev/null
+++ b/Platform/Marvell/Library/ComPhyLib/ComPhyCp110.c
@@ -0,0 +1,1853 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must Retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#include "ComPhyLib.h"
+#include <Library/MvHwDescLib.h>
+
+#define SD_LANE_ADDR_WIDTH          0x1000
+#define HPIPE_ADDR_OFFSET           0x800
+#define COMPHY_ADDR_LANE_WIDTH      0x28
+#define SD_ADDR(base, Lane)         (base + SD_LANE_ADDR_WIDTH * Lane)
+#define HPIPE_ADDR(base, Lane)      (SD_ADDR(base, Lane) + HPIPE_ADDR_OFFSET)
+#define COMPHY_ADDR(base, Lane)     (base + COMPHY_ADDR_LANE_WIDTH * Lane)
+
+DECLARE_A7K8K_NONDISCOVERABLE_TEMPLATE;
+
+/*
+ * For CP-110 we have 2 Selector registers "PHY Selectors"
+ * and " PIPE Selectors".
+ * PIPE selector include USB and PCIe options.
+ * PHY selector include the Ethernet and SATA options, every Ethernet option
+ * has different options, for example: serdes Lane2 have option Eth_port_0
+ * that include (SGMII0, RXAUI0, SFI)
+ */
+COMPHY_MUX_DATA Cp110ComPhyMuxData[] = {
+  /* Lane 0 */
+  {4, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_SGMII1, 0x1},
+       {COMPHY_TYPE_SATA1, 0x4}, {COMPHY_TYPE_SATA3, 0x4}}},
+  /* Lane 1 */
+  {4, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_SGMII2, 0x1},
+       {COMPHY_TYPE_SATA0, 0x4}, {COMPHY_TYPE_SATA2, 0x4}}},
+  /* Lane 2 */
+  {6, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_SGMII0, 0x1},
+       {COMPHY_TYPE_RXAUI0, 0x1}, {COMPHY_TYPE_SFI, 0x1},
+       {COMPHY_TYPE_SATA0, 0x4}, {COMPHY_TYPE_SATA2, 0x4}}},
+  /* Lane 3 */
+  {8, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_RXAUI1, 0x1},
+       {COMPHY_TYPE_SGMII1, 0x2}, {COMPHY_TYPE_SATA1, 0x4},
+       {COMPHY_TYPE_SATA3, 0x4}}},
+  /* Lane 4 */
+  {7, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_SGMII0, 0x2},
+       {COMPHY_TYPE_RXAUI0, 0x2}, {COMPHY_TYPE_SFI, 0x2},
+       {COMPHY_TYPE_SGMII1, 0x1}}},
+  /* Lane 5 */
+  {6, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_SGMII2, 0x1},
+       {COMPHY_TYPE_RXAUI1, 0x2}, {COMPHY_TYPE_SATA1, 0x4},
+       {COMPHY_TYPE_SATA3, 0x4}}},
+};
+
+COMPHY_MUX_DATA Cp110ComPhyPipeMuxData[] = {
+  /* Lane 0 */
+  {2, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_PCIE0, 0x4}}},
+  /* Lane 1 */
+  {4, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_USB3_HOST0, 0x1},
+       {COMPHY_TYPE_USB3_DEVICE, 0x2}, {COMPHY_TYPE_PCIE0, 0x4}}},
+  /* Lane 2 */
+  {3, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_USB3_HOST0, 0x1},
+       {COMPHY_TYPE_PCIE0, 0x4}}},
+  /* Lane 3 */
+  {3, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_USB3_HOST1, 0x1},
+       {COMPHY_TYPE_PCIE0, 0x4}}},
+  /* Lane 4 */
+  {4, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_USB3_HOST1, 0x1},
+       {COMPHY_TYPE_USB3_DEVICE, 0x2}, {COMPHY_TYPE_PCIE1, 0x4}}},
+  /* Lane 5 */
+  {2, {{COMPHY_TYPE_UNCONNECTED, 0x0}, {COMPHY_TYPE_PCIE2, 0x4}}},
+};
+
+STATIC
+VOID
+ComPhyPcieRFUConfiguration (
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* RFU configurations - hard reset ComPhy */
+  Mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+  Data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+  Data |= 0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+  Mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+  Mask |= COMMON_PHY_PHY_MODE_MASK;
+  Data |= 0x0 << COMMON_PHY_PHY_MODE_OFFSET;
+  RegSet (ComPhyAddr + COMMON_PHY_CFG1_REG, Data, Mask);
+
+  /* Release from hard reset */
+  Mask = COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+  Data = 0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+  Mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+  Data |= 0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+  RegSet (ComPhyAddr + COMMON_PHY_CFG1_REG, Data, Mask);
+
+  /* Wait 1ms - until band gap and ref clock ready */
+  MicroSecondDelay (1000);
+  MemoryFence ();
+}
+
+STATIC
+VOID
+ComPhyPciePhyConfiguration (
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr,
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  UINT32 Mask, Data, PcieClk = 0;
+
+  /* Set PIPE soft reset */
+  Mask = HPIPE_RST_CLK_CTRL_PIPE_RST_MASK;
+  Data = 0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET;
+
+  /* Set PHY Datapath width mode for V0 */
+  Mask |= HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK;
+  Data |= 0x1 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET;
+
+  /* Set Data bus width USB mode for V0 */
+  Mask |= HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK;
+  Data |= 0x0 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET;
+
+  /* Set CORE_CLK output frequency for 250Mhz */
+  Mask |= HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK;
+  Data |= 0x0 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET;
+  RegSet (HpipeAddr + HPIPE_RST_CLK_CTRL_REG, Data, Mask);
+
+  /* Set PLL ready delay for 0x2 */
+  RegSet (HpipeAddr + HPIPE_CLK_SRC_LO_REG,
+    0x2 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET,
+    HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK);
+
+  /* Set PIPE mode interface to PCIe3 - 0x1 */
+  RegSet (HpipeAddr + HPIPE_CLK_SRC_HI_REG,
+    0x1 << HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET, HPIPE_CLK_SRC_HI_MODE_PIPE_MASK);
+
+  /* Config update polarity equalization */
+  RegSet (HpipeAddr + HPIPE_LANE_EQ_CFG1_REG,
+    0x1 << HPIPE_CFG_UPDATE_POLARITY_OFFSET, HPIPE_CFG_UPDATE_POLARITY_MASK);
+
+  /* Set PIPE version 4 to mode enable */
+  RegSet (HpipeAddr + HPIPE_DFE_CTRL_28_REG,
+    0x1 << HPIPE_DFE_CTRL_28_PIPE4_OFFSET, HPIPE_DFE_CTRL_28_PIPE4_MASK);
+
+  /* Enable PIN clock 100M_125M */
+  Mask = HPIPE_MISC_CLK100M_125M_MASK;
+  Data = 0x1 << HPIPE_MISC_CLK100M_125M_OFFSET;
+
+  /* Set PIN_TXDCLK_2X Clock Frequency Selection for outputs 500MHz clock */
+  Mask |= HPIPE_MISC_TXDCLK_2X_MASK;
+  Data |= 0x0 << HPIPE_MISC_TXDCLK_2X_OFFSET;
+
+  /* Enable 500MHz Clock */
+  Mask |= HPIPE_MISC_CLK500_EN_MASK;
+  Data |= 0x1 << HPIPE_MISC_CLK500_EN_OFFSET;
+
+  if (PcieClk) {
+    /* Set reference clock comes from group 1 */
+    Mask |= HPIPE_MISC_REFCLK_SEL_MASK;
+    Data |= 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+  } else {
+    /* Set reference clock comes from group 2 */
+    Mask |= HPIPE_MISC_REFCLK_SEL_MASK;
+    Data |= 0x1 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+  }
+
+  /* Force ICP */
+  Mask |= HPIPE_MISC_ICP_FORCE_MASK;
+  Data |= 0x1 << HPIPE_MISC_ICP_FORCE_OFFSET;
+  RegSet (HpipeAddr + HPIPE_MISC_REG, Data, Mask);
+
+  if (PcieClk) {
+    /* Set reference frequcency select - 0x2 for 25MHz*/
+    Mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+    Data = 0x2 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+  } else {
+    /* Set reference frequcency select - 0x0 for 100MHz*/
+    Mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+    Data = 0x0 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+  }
+
+  /* Set PHY mode to PCIe */
+  Mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+  Data |= 0x3 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+  RegSet (HpipeAddr + HPIPE_PWR_PLL_REG, Data, Mask);
+
+  /*
+   * Set the amount of time spent in the LoZ state - set
+   * for 0x7 only if the PCIe clock is output
+   */
+  if (PcieClk)
+    RegSet (HpipeAddr + HPIPE_GLOBAL_PM_CTRL,
+      0x7 << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET,
+      HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK);
+
+  /* Set Maximal PHY Generation Setting (8Gbps) */
+  Mask = HPIPE_INTERFACE_GEN_MAX_MASK;
+  Data = 0x2 << HPIPE_INTERFACE_GEN_MAX_OFFSET;
+  /* Bypass frame detection and sync detection for RX DATA */
+  Mask |= HPIPE_INTERFACE_DET_BYPASS_MASK;
+  Data |= 0x1 << HPIPE_INTERFACE_DET_BYPASS_OFFSET;
+  /* Set Link Train Mode (Tx training control pins are used) */
+  Mask |= HPIPE_INTERFACE_LINK_TRAIN_MASK;
+  Data |= 0x1 << HPIPE_INTERFACE_LINK_TRAIN_OFFSET;
+  RegSet (HpipeAddr + HPIPE_INTERFACE_REG, Data, Mask);
+
+  /* Set Idle_sync enable */
+  Mask = HPIPE_PCIE_IDLE_SYNC_MASK;
+  Data = 0x1 << HPIPE_PCIE_IDLE_SYNC_OFFSET;
+
+  /* Select bits for PCIE Gen3(32bit) */
+  Mask |= HPIPE_PCIE_SEL_BITS_MASK;
+  Data |= 0x2 << HPIPE_PCIE_SEL_BITS_OFFSET;
+  RegSet (HpipeAddr + HPIPE_PCIE_REG0, Data, Mask);
+
+  /* Enable Tx_adapt_g1 */
+  Mask = HPIPE_TX_TRAIN_CTRL_G1_MASK;
+  Data = 0x1 << HPIPE_TX_TRAIN_CTRL_G1_OFFSET;
+
+  /* Enable Tx_adapt_gn1 */
+  Mask |= HPIPE_TX_TRAIN_CTRL_GN1_MASK;
+  Data |= 0x1 << HPIPE_TX_TRAIN_CTRL_GN1_OFFSET;
+
+  /* Disable Tx_adapt_g0 */
+  Mask |= HPIPE_TX_TRAIN_CTRL_G0_MASK;
+  Data |= 0x0 << HPIPE_TX_TRAIN_CTRL_G0_OFFSET;
+  RegSet (HpipeAddr + HPIPE_TX_TRAIN_CTRL_REG, Data, Mask);
+
+  /* Set reg_tx_train_chk_init */
+  Mask = HPIPE_TX_TRAIN_CHK_INIT_MASK;
+  Data = 0x0 << HPIPE_TX_TRAIN_CHK_INIT_OFFSET;
+
+  /* Enable TX_COE_FM_PIN_PCIE3_EN */
+  Mask |= HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_MASK;
+  Data |= 0x1 << HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET;
+  RegSet (HpipeAddr + HPIPE_TX_TRAIN_REG, Data, Mask);
+}
+
+STATIC
+VOID
+ComPhyPcieSetAnalogParameters (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  UINT32 Data, Mask;
+
+  /* Set preset sweep configurations */
+  Mask = HPIPE_TX_TX_STATUS_CHECK_MODE_MASK |
+         HPIPE_TX_NUM_OF_PRESET_MASK |
+         HPIPE_TX_SWEEP_PRESET_EN_MASK;
+  Data = (0x1 << HPIPE_TX_STATUS_CHECK_MODE_OFFSET) |
+         (0x7 << HPIPE_TX_NUM_OF_PRESET_OFFSET) |
+         (0x1 << HPIPE_TX_SWEEP_PRESET_EN_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_TX_TRAIN_CTRL_11_REG, ~Mask, Data);
+
+  /* Tx train start configuration */
+  Mask = HPIPE_TX_TRAIN_START_SQ_EN_MASK |
+         HPIPE_TX_TRAIN_START_FRM_DET_EN_MASK |
+         HPIPE_TX_TRAIN_START_FRM_LOCK_EN_MASK |
+         HPIPE_TX_TRAIN_WAIT_TIME_EN_MASK;
+  Data = (0x1 << HPIPE_TX_TRAIN_START_SQ_EN_OFFSET) |
+         (0x1 << HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_TX_TRAIN_CTRL_5_REG, ~Mask, Data);
+
+  /* Enable Tx train P2P */
+  MmioOr32 (HpipeAddr + HPIPE_TX_TRAIN_CTRL_0_REG, HPIPE_TX_TRAIN_P2P_HOLD_MASK);
+
+  /* Configure Tx train timeout */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_TX_TRAIN_CTRL_4_REG,
+          ~HPIPE_TRX_TRAIN_TIMER_MASK,
+          0x17 << HPIPE_TRX_TRAIN_TIMER_OFFSET
+          );
+
+  /* Disable G0/G1/GN1 adaptation */
+  MmioAnd32 (
+          HpipeAddr + HPIPE_TX_TRAIN_CTRL_REG,
+          ~(HPIPE_TX_TRAIN_CTRL_G1_MASK | HPIPE_TX_TRAIN_CTRL_GN1_MASK | HPIPE_TX_TRAIN_CTRL_G0_OFFSET)
+          );
+
+  /* Disable DTL frequency loop */
+  MmioAnd32 (HpipeAddr + HPIPE_PWR_CTR_DTL_REG, ~HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK);
+
+  /* Configure Generation 3 DFE */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G3_SET4_REG,
+          ~HPIPE_GX_SET4_DFE_RES_MASK,
+          0x3 << HPIPE_GX_SET4_DFE_RES_OFFSET
+          );
+
+  /* Use TX/RX training result for DFE */
+  MmioAnd32 (HpipeAddr + HPIPE_DFE_REG0, ~HPIPE_DFE_RES_FORCE_MASK);
+
+  /* Configure initial and final coefficient value for receiver */
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET1_REG,  ~Mask, Data);
+  Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK |
+         HPIPE_GX_SET1_RX_SELMUPP_MASK |
+         HPIPE_GX_SET1_SAMPLER_INPAIRX2_EN_MASK;
+  Data = 0x1 | (0x1 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET1_REG, ~Mask, Data);
+
+  /* Trigger sampler 5us enable pulse */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_SAMPLER_MASK,
+          0x1 << HPIPE_SAMPLER_OFFSET
+          );
+  MicroSecondDelay (5);
+  MmioAnd32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_SAMPLER_MASK
+          );
+
+  /* FFE resistor tuning for different bandwidth  */
+  Mask = HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_MASK |
+         HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_MASK;
+  Data = (0x1 << HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_OFFSET) |
+         (0x3 << HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET3_REG, ~Mask, Data);
+
+  /* Pattern lock lost timeout disable */
+  MmioAnd32 (HpipeAddr + HPIPE_FRAME_DETECT_CTRL_3_REG, ~HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_MASK);
+
+  /* Configure DFE adaptations */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_CDR_CONTROL_REG,
+          ~(HPIPE_CDR_MAX_DFE_ADAPT_1_MASK | HPIPE_CDR_MAX_DFE_ADAPT_0_MASK | HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK),
+          0x1 << HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET
+          );
+  MmioAnd32 (HpipeAddr + HPIPE_DFE_CONTROL_REG, ~HPIPE_DFE_TX_MAX_DFE_ADAPT_MASK);
+
+  /* Hpipe Generation 2 setting 1*/
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G2_SET1_REG,
+          ~(HPIPE_GX_SET1_RX_SELMUPI_MASK | HPIPE_GX_SET1_RX_SELMUPP_MASK | HPIPE_GX_SET1_RX_SELMUFI_MASK),
+          0x1 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET
+          );
+
+  /* DFE enable */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G2_SET4_REG,
+          ~HPIPE_GX_SET4_DFE_RES_MASK,
+          0x3 << HPIPE_GX_SET4_DFE_RES_OFFSET
+          );
+
+  /* Configure DFE Resolution */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_LANE_CFG4_REG,
+          ~HPIPE_LANE_CFG4_DFE_EN_SEL_MASK,
+          0x1 << HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET
+          );
+
+  /* VDD calibration control */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_VDD_CAL_CTRL_REG,
+          ~HPIPE_EXT_SELLV_RXSAMPL_MASK,
+          0x16 << HPIPE_EXT_SELLV_RXSAMPL_OFFSET
+          );
+
+  /* Set PLL Charge-pump Current Control */
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET5_REG, ~HPIPE_GX_SET5_ICP_MASK, 0x4);
+
+  /* Set lane rqualization remote setting */
+  Mask = HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_MASK |
+         HPIPE_LANE_CFG_FOM_ONLY_MODE_MASK |
+         HPIPE_LANE_CFG_FOM_PRESET_VECTOR_MASK;
+  Data = (0x1 << HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET) |
+         (0x1 << HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET) |
+         (0x2 << HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_LANE_EQ_REMOTE_SETTING_REG, ~Mask, Data);
+
+  /* Set phy in root complex mode */
+  MmioOr32 (HpipeAddr + HPIPE_LANE_EQU_CONFIG_0_REG, HPIPE_CFG_PHY_RC_EP_MASK);
+}
+
+STATIC
+VOID
+ComPhyPciePhyPowerUp (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  /* Release from PIPE soft reset */
+  RegSet (HpipeAddr + HPIPE_RST_CLK_CTRL_REG,
+    0x0 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET,
+    HPIPE_RST_CLK_CTRL_PIPE_RST_MASK);
+
+  /* Wait 15ms - for ComPhy calibration done */
+  MicroSecondDelay (15000);
+  MemoryFence ();
+}
+
+STATIC
+EFI_STATUS
+ComPhyPcieCheckPll (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  EFI_STATUS Status = EFI_SUCCESS;
+  UINT32 Data;
+
+  /* Read Lane status */
+  Data = MmioRead32 (HpipeAddr + HPIPE_LANE_STATUS0_REG);
+  if ((Data & HPIPE_LANE_STATUS0_PCLK_EN_MASK) == 0) {
+    DEBUG((DEBUG_INFO, "ComPhy: Read from reg = %p - value = 0x%x\n",
+      HpipeAddr + HPIPE_LANE_STATUS0_REG, Data));
+    DEBUG((DEBUG_INFO, "ComPhy: HPIPE_LANE_STATUS0_PCLK_EN_MASK is 0\n"));
+    Status = EFI_D_ERROR;
+  }
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+ComPhyPciePowerUp (
+  IN UINT32 Lane,
+  IN UINT32 PcieBy4,
+  IN EFI_PHYSICAL_ADDRESS HpipeBase,
+  IN EFI_PHYSICAL_ADDRESS ComPhyBase
+  )
+{
+  EFI_STATUS Status = EFI_SUCCESS;
+  EFI_PHYSICAL_ADDRESS HpipeAddr = HPIPE_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS ComPhyAddr = COMPHY_ADDR(ComPhyBase, Lane);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: RFU configurations - hard reset ComPhy\n"));
+
+  ComPhyPcieRFUConfiguration (ComPhyAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: ComPhy configuration\n"));
+
+  ComPhyPciePhyConfiguration (ComPhyAddr, HpipeAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Set analog paramters\n"));
+
+  ComPhyPcieSetAnalogParameters (HpipeAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: ComPhy power up\n"));
+
+  ComPhyPciePhyPowerUp (HpipeAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Check PLL\n"));
+
+  Status = ComPhyPcieCheckPll (HpipeAddr);
+
+  return Status;
+}
+
+STATIC
+VOID
+ComPhyUsb3RFUConfiguration (
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* RFU configurations - hard reset ComPhy */
+  Mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+  Data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+  Data |= 0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+  Mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+  Mask |= COMMON_PHY_PHY_MODE_MASK;
+  Data |= 0x1 << COMMON_PHY_PHY_MODE_OFFSET;
+  RegSet (ComPhyAddr + COMMON_PHY_CFG1_REG, Data, Mask);
+
+  /* Release from hard reset */
+  Mask = COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+  Data = 0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+  Mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+  Data |= 0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+  RegSet (ComPhyAddr + COMMON_PHY_CFG1_REG, Data, Mask);
+
+  /* Wait 1ms - until band gap and ref clock ready */
+  MicroSecondDelay (1000);
+  MemoryFence ();
+}
+
+STATIC
+VOID
+ComPhyUsb3PhyConfiguration (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* Set PIPE soft reset */
+  Mask = HPIPE_RST_CLK_CTRL_PIPE_RST_MASK;
+  Data = 0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET;
+
+  /* Set PHY Datapath width mode for V0 */
+  Mask |= HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK;
+  Data |= 0x0 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET;
+
+  /* Set Data bus width USB mode for V0 */
+  Mask |= HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK;
+  Data |= 0x0 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET;
+
+  /* Set CORE_CLK output frequency for 250Mhz */
+  Mask |= HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK;
+  Data |= 0x0 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET;
+  RegSet (HpipeAddr + HPIPE_RST_CLK_CTRL_REG, Data, Mask);
+
+  /* Set PLL ready delay for 0x2 */
+  RegSet (HpipeAddr + HPIPE_CLK_SRC_LO_REG,
+    0x2 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET,
+    HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK);
+
+  /* Set reference clock to come from group 1 - 25Mhz */
+  RegSet (HpipeAddr + HPIPE_MISC_REG, 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET,
+    HPIPE_MISC_REFCLK_SEL_MASK);
+
+  /* Set reference frequcency select - 0x2 */
+  Mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+  Data = 0x2 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+
+  /* Set PHY mode to USB - 0x5 */
+  Mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+  Data |= 0x5 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+  RegSet (HpipeAddr + HPIPE_PWR_PLL_REG, Data, Mask);
+
+  /* Set the amount of time spent in the LoZ state - set for 0x7 */
+  RegSet (HpipeAddr + HPIPE_GLOBAL_PM_CTRL,
+    0x7 << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET,
+    HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK);
+
+  /* Set max PHY generation setting - 5Gbps */
+  RegSet (HpipeAddr + HPIPE_INTERFACE_REG,
+    0x1 << HPIPE_INTERFACE_GEN_MAX_OFFSET, HPIPE_INTERFACE_GEN_MAX_MASK);
+
+  /* Set select Data width 20Bit (SEL_BITS[2:0]) */
+  RegSet (HpipeAddr + HPIPE_LOOPBACK_REG,
+    0x1 << HPIPE_LOOPBACK_SEL_OFFSET, HPIPE_LOOPBACK_SEL_MASK);
+}
+
+STATIC
+VOID
+ComPhyUsb3SetAnalogParameters (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  UINT32 Data, Mask;
+
+  /* Set Pin DFE_PAT_DIS -> Bit[1]: PIN_DFE_PAT_DIS = 0x0 */
+  Mask = HPIPE_LANE_CFG4_DFE_CTRL_MASK;
+  Data = 0x1 << HPIPE_LANE_CFG4_DFE_CTRL_OFFSET;
+
+  /* Set Override PHY DFE control pins for 0x1 */
+  Mask |= HPIPE_LANE_CFG4_DFE_OVER_MASK;
+  Data |= 0x1 << HPIPE_LANE_CFG4_DFE_OVER_OFFSET;
+
+  /* Set Spread Spectrum Clock Enable fot 0x1 */
+  Mask |= HPIPE_LANE_CFG4_SSC_CTRL_MASK;
+  Data |= 0x1 << HPIPE_LANE_CFG4_SSC_CTRL_OFFSET;
+  RegSet (HpipeAddr + HPIPE_LANE_CFG4_REG, Data, Mask);
+}
+
+STATIC
+UINTN
+ComphyUsb3PowerUp (
+  UINT32 Lane,
+  EFI_PHYSICAL_ADDRESS HpipeBase,
+  EFI_PHYSICAL_ADDRESS ComPhyBase
+  )
+{
+  EFI_STATUS Status = EFI_SUCCESS;
+  UINT32 Data;
+  EFI_PHYSICAL_ADDRESS HpipeAddr = HPIPE_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS ComPhyAddr = COMPHY_ADDR(ComPhyBase, Lane);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: RFU configurations - hard reset ComPhy\n"));
+
+  ComPhyUsb3RFUConfiguration (ComPhyAddr);
+
+  /* Start ComPhy Configuration */
+  DEBUG((DEBUG_INFO, "stage: Comphy configuration\n"));
+
+  ComPhyUsb3PhyConfiguration (HpipeAddr);
+
+  /* Start analog paramters from ETP(HW) */
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Analog paramters from ETP(HW)\n"));
+
+  ComPhyUsb3SetAnalogParameters (HpipeAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Comphy power up\n"));
+
+  /* Release from PIPE soft reset */
+  RegSet (HpipeAddr + HPIPE_RST_CLK_CTRL_REG,
+    0x0 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET,
+    HPIPE_RST_CLK_CTRL_PIPE_RST_MASK);
+
+  /* Wait 15ms - for ComPhy calibration done */
+  MicroSecondDelay (15000);
+  MemoryFence ();
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Check PLL\n"));
+
+  /* Read Lane status */
+  Data = MmioRead32 (HpipeAddr + HPIPE_LANE_STATUS0_REG);
+  if ((Data & HPIPE_LANE_STATUS0_PCLK_EN_MASK) == 0) {
+    DEBUG((DEBUG_ERROR, "ComPhy: HPIPE_LANE_STATUS0_PCLK_EN_MASK is 0\n"));
+    Status = EFI_D_ERROR;
+  }
+
+  return Status;
+}
+
+STATIC
+UINT32
+PollingWithTimeout (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Val,
+  IN UINT32 Mask,
+  IN UINT64 Usec_timeout
+  )
+{
+  UINT32 Data;
+
+  do {
+    MicroSecondDelay(1);
+    Data = MmioRead32(Addr) & Mask;
+  } while (Data != Val  && --Usec_timeout > 0);
+
+  if (Usec_timeout == 0)
+    return Data;
+  return 0;
+}
+
+STATIC
+VOID
+ComPhySataMacPowerDown (
+  IN EFI_PHYSICAL_ADDRESS SataBase
+)
+{
+  UINT32 Mask, Data;
+
+  /*
+   * MAC configuration - power down ComPhy
+   * Use indirect address for vendor specific SATA control register
+   */
+  RegSet (SataBase + SATA3_VENDOR_ADDRESS,
+    SATA_CONTROL_REG << SATA3_VENDOR_ADDR_OFSSET, SATA3_VENDOR_ADDR_MASK);
+
+  /* SATA 0 power down */
+  Mask = SATA3_CTRL_SATA0_PD_MASK;
+  Data = 0x1 << SATA3_CTRL_SATA0_PD_OFFSET;
+
+  /* SATA 1 power down */
+  Mask |= SATA3_CTRL_SATA1_PD_MASK;
+  Data |= 0x1 << SATA3_CTRL_SATA1_PD_OFFSET;
+
+  /* SATA SSU disable */
+  Mask |= SATA3_CTRL_SATA1_ENABLE_MASK;
+  Data |= 0x0 << SATA3_CTRL_SATA1_ENABLE_OFFSET;
+
+  /* SATA port 1 disable */
+  Mask |= SATA3_CTRL_SATA_SSU_MASK;
+  Data |= 0x0 << SATA3_CTRL_SATA_SSU_OFFSET;
+  RegSet (SataBase + SATA3_VENDOR_DATA, Data, Mask);
+}
+
+STATIC
+VOID
+ComPhySataRFUConfiguration (
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* RFU configurations - hard reset ComPhy */
+  Mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+  Data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PWR_ON_RESET_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET;
+  Mask |= COMMON_PHY_CFG1_CORE_RSTN_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET;
+  RegSet (ComPhyAddr + COMMON_PHY_CFG1_REG, Data, Mask);
+
+  /* Set select Data  width 40Bit - SATA mode only */
+  RegSet (ComPhyAddr + COMMON_PHY_CFG6_REG,
+    0x1 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET, COMMON_PHY_CFG6_IF_40_SEL_MASK);
+
+  /* Release from hard reset in SD external */
+  Mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+  Data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+  Data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+  RegSet (SdIpAddr + SD_EXTERNAL_CONFIG1_REG, Data, Mask);
+
+  /* Wait 1ms - until band gap and ref clock ready */
+  MicroSecondDelay (1000);
+  MemoryFence ();
+}
+
+STATIC
+VOID
+ComPhySataPhyConfiguration (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* Set reference clock to comes from group 1 - choose 25Mhz */
+  RegSet (HpipeAddr + HPIPE_MISC_REG,
+    0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET, HPIPE_MISC_REFCLK_SEL_MASK);
+
+  /* Reference frequency select set 1 (for SATA = 25Mhz) */
+  Mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+  Data = 0x1 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+
+  /* PHY mode select (set SATA = 0x0 */
+  Mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+  Data |= 0x0 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+  RegSet (HpipeAddr + HPIPE_PWR_PLL_REG, Data, Mask);
+
+  /* Set max PHY generation setting - 6Gbps */
+  RegSet (HpipeAddr + HPIPE_INTERFACE_REG,
+    0x2 << HPIPE_INTERFACE_GEN_MAX_OFFSET, HPIPE_INTERFACE_GEN_MAX_MASK);
+
+  /* Set select Data  width 40Bit (SEL_BITS[2:0]) */
+  RegSet (HpipeAddr + HPIPE_LOOPBACK_REG,
+    0x2 << HPIPE_LOOPBACK_SEL_OFFSET, HPIPE_LOOPBACK_SEL_MASK);
+}
+
+STATIC
+VOID
+ComPhySataSetAnalogParameters (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* Hpipe Generation 1 settings 1 */
+  Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK |
+         HPIPE_GX_SET1_RX_SELMUPP_MASK |
+         HPIPE_GX_SET1_RX_SELMUFI_MASK |
+         HPIPE_GX_SET1_RX_SELMUFF_MASK |
+         HPIPE_GX_SET1_RX_DIGCK_DIV_MASK;
+  Data = (0x1 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET) |
+         (0x3 << HPIPE_GX_SET1_RX_SELMUFF_OFFSET) |
+         (0x1 << HPIPE_GX_SET1_RX_DIGCK_DIV_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET1_REG, ~Mask, Data);
+
+  /* Hpipe Generation 1 settings 3 */
+  Mask = HPIPE_GX_SET3_FFE_CAP_SEL_MASK |
+         HPIPE_GX_SET3_FFE_RES_SEL_MASK |
+         HPIPE_GX_SET3_FFE_SETTING_FORCE_MASK |
+         HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_MASK |
+         HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_MASK;
+  Data = 0xf |
+         (0x2 << HPIPE_GX_SET3_FFE_RES_SEL_OFFSET) |
+         (0x1 << HPIPE_GX_SET3_FFE_SETTING_FORCE_OFFSET) |
+         (0x1 << HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_OFFSET) |
+         (0x1 << HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET3_REG, ~Mask, Data);
+
+  /* Hpipe Generation 2 settings 1 */
+  Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK |
+         HPIPE_GX_SET1_RX_SELMUPP_MASK |
+         HPIPE_GX_SET1_RX_SELMUFI_MASK |
+         HPIPE_GX_SET1_RX_SELMUFF_MASK |
+         HPIPE_GX_SET1_RX_DIGCK_DIV_MASK;
+  Data = (0x1 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET) |
+         (0x3 << HPIPE_GX_SET1_RX_SELMUFF_OFFSET) |
+         (0x1 << HPIPE_GX_SET1_RX_DIGCK_DIV_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G2_SET1_REG, ~Mask, Data);
+
+  /* Hpipe Generation 3 settings 1 */
+  Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK |
+         HPIPE_GX_SET1_RX_SELMUPP_MASK |
+         HPIPE_GX_SET1_RX_SELMUFI_MASK |
+         HPIPE_GX_SET1_RX_SELMUFF_MASK |
+         HPIPE_GX_SET1_RX_DFE_EN_MASK |
+         HPIPE_GX_SET1_RX_DIGCK_DIV_MASK |
+         HPIPE_GX_SET1_SAMPLER_INPAIRX2_EN_MASK;
+  Data = 0x2 |
+         (0x2 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET) |
+         (0x3 << HPIPE_GX_SET1_RX_SELMUFI_OFFSET) |
+         (0x3 << HPIPE_GX_SET1_RX_SELMUFF_OFFSET) |
+         (0x1 << HPIPE_GX_SET1_RX_DFE_EN_OFFSET) |
+         (0x2 << HPIPE_GX_SET1_RX_DIGCK_DIV_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET1_REG, ~Mask, Data);
+
+  /* DTL Control */
+  Mask = HPIPE_PWR_CTR_DTL_SQ_DET_EN_MASK |
+         HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_MASK |
+         HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK |
+         HPIPE_PWR_CTR_DTL_CLAMPING_SEL_MASK |
+         HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_MASK |
+         HPIPE_PWR_CTR_DTL_CLK_MODE_MASK |
+         HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_MASK;
+  Data = 0x1 |
+         (0x1 << HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_OFFSET) |
+         (0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET) |
+         (0x1 << HPIPE_PWR_CTR_DTL_CLAMPING_SEL_OFFSET) |
+         (0x1 << HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_OFFSET) |
+         (0x1 << HPIPE_PWR_CTR_DTL_CLK_MODE_OFFSET) |
+         (0x1 << HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_PWR_CTR_DTL_REG, ~Mask, Data);
+
+  /* Trigger sampler enable pulse (by toggling the bit) */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_SAMPLER_MASK,
+          0x1 << HPIPE_SAMPLER_OFFSET
+          );
+  MmioAnd32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_SAMPLER_MASK
+          );
+
+  /* VDD Calibration Control 3 */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_VDD_CAL_CTRL_REG,
+          ~HPIPE_EXT_SELLV_RXSAMPL_MASK,
+          0x10 << HPIPE_EXT_SELLV_RXSAMPL_OFFSET
+          );
+
+  /* DFE Resolution Control */
+  MmioOr32 (HpipeAddr + HPIPE_DFE_REG0, HPIPE_DFE_RES_FORCE_MASK);
+
+  /* DFE F3-F5 Coefficient Control */
+  MmioAnd32 (
+          HpipeAddr + HPIPE_DFE_F3_F5_REG,
+          ~(HPIPE_DFE_F3_F5_DFE_EN_MASK | HPIPE_DFE_F3_F5_DFE_CTRL_MASK)
+          );
+
+  /* Hpipe Generation 3 settings 3 */
+  Mask = HPIPE_GX_SET3_FFE_CAP_SEL_MASK |
+         HPIPE_GX_SET3_FFE_RES_SEL_MASK |
+         HPIPE_GX_SET3_FFE_SETTING_FORCE_MASK |
+         HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_MASK |
+         HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_MASK;
+  Data = 0xf |
+         (0x4 << HPIPE_GX_SET3_FFE_RES_SEL_OFFSET) |
+         (0x1 << HPIPE_GX_SET3_FFE_SETTING_FORCE_OFFSET) |
+         (0x1 << HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_OFFSET) |
+         (0x3 << HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET3_REG, ~Mask, Data);
+
+  /* Hpipe Generation 3 settings 4 */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G3_SET4_REG,
+          ~HPIPE_GX_SET4_DFE_RES_MASK,
+          0x2 << HPIPE_GX_SET4_DFE_RES_OFFSET
+          );
+
+  /* Offset Phase Control - force offset and toggle 'valid' bit */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_PHASE_CONTROL_REG,
+          ~(HPIPE_OS_PH_OFFSET_MASK | HPIPE_OS_PH_OFFSET_FORCE_MASK),
+          0x5c | (0x1 << HPIPE_OS_PH_OFFSET_FORCE_OFFSET)
+          );
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_PHASE_CONTROL_REG,
+          ~HPIPE_OS_PH_VALID_MASK,
+          0x1 << HPIPE_OS_PH_VALID_OFFSET
+          );
+  MmioAnd32 (
+          HpipeAddr + HPIPE_PHASE_CONTROL_REG,
+          ~HPIPE_OS_PH_VALID_MASK
+          );
+
+  /* Set G1 TX amplitude and TX post emphasis value */
+  Mask = HPIPE_GX_SET0_TX_AMP_MASK |
+         HPIPE_GX_SET0_TX_AMP_ADJ_MASK |
+         HPIPE_GX_SET0_TX_EMPH1_MASK |
+         HPIPE_GX_SET0_TX_EMPH1_EN_MASK;
+  Data = (0x8 << HPIPE_GX_SET0_TX_AMP_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_AMP_ADJ_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_EMPH1_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_EMPH1_EN_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET0_REG, ~Mask, Data);
+
+  /* Set G2 TX amplitude and TX post emphasis value */
+  Mask = HPIPE_GX_SET0_TX_AMP_MASK |
+         HPIPE_GX_SET0_TX_AMP_ADJ_MASK |
+         HPIPE_GX_SET0_TX_EMPH1_MASK |
+         HPIPE_GX_SET0_TX_EMPH1_EN_MASK;
+  Data = (0xa << HPIPE_GX_SET0_TX_AMP_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_AMP_ADJ_OFFSET) |
+         (0x2 << HPIPE_GX_SET0_TX_EMPH1_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_EMPH1_EN_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G2_SET0_REG, ~Mask, Data);
+
+  /* Set G3 TX amplitude and TX post emphasis value */
+  Mask = HPIPE_GX_SET0_TX_AMP_MASK |
+         HPIPE_GX_SET0_TX_AMP_ADJ_MASK |
+         HPIPE_GX_SET0_TX_EMPH1_MASK |
+         HPIPE_GX_SET0_TX_EMPH1_EN_MASK |
+         HPIPE_GX_SET0_TX_SLEW_RATE_SEL_MASK |
+         HPIPE_GX_SET0_TX_SLEW_CTRL_EN_MASK;
+  Data = (0xe << HPIPE_GX_SET0_TX_AMP_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_AMP_ADJ_OFFSET) |
+         (0x6 << HPIPE_GX_SET0_TX_EMPH1_OFFSET) |
+         (0x1 << HPIPE_GX_SET0_TX_EMPH1_EN_OFFSET) |
+         (0x4 << HPIPE_GX_SET0_TX_SLEW_RATE_SEL_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G3_SET0_REG, ~Mask, Data);
+
+  /* SERDES External Configuration 2 register - enable spread spectrum clock */
+  MmioOr32 (SdIpAddr + SD_EXTERNAL_CONFIG2_REG, SD_EXTERNAL_CONFIG2_SSC_ENABLE_MASK);
+
+  /* DFE reset sequence */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_PWR_CTR_REG,
+          ~HPIPE_PWR_CTR_RST_DFE_MASK,
+          0x1
+          );
+  MmioAnd32 (
+          HpipeAddr + HPIPE_PWR_CTR_REG,
+          ~HPIPE_PWR_CTR_RST_DFE_MASK
+          );
+
+  /* SW reset for interupt logic */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_PWR_CTR_REG,
+          ~HPIPE_PWR_CTR_SFT_RST_MASK,
+          0x1 << HPIPE_PWR_CTR_SFT_RST_OFFSET
+          );
+  MmioAnd32 (
+          HpipeAddr + HPIPE_PWR_CTR_REG,
+          ~HPIPE_PWR_CTR_SFT_RST_MASK
+          );
+}
+
+STATIC
+VOID
+ComPhySataPhyPowerUp (
+  IN EFI_PHYSICAL_ADDRESS SataBase
+)
+{
+  UINT32 Data, Mask;
+
+  /*
+   * MAC configuration - power up ComPhy - power up PLL/TX/RX
+   * Use indirect address for vendor specific SATA control register
+   */
+  RegSet (SataBase + SATA3_VENDOR_ADDRESS,
+    SATA_CONTROL_REG << SATA3_VENDOR_ADDR_OFSSET, SATA3_VENDOR_ADDR_MASK);
+
+  /* SATA 0 power up */
+  Mask = SATA3_CTRL_SATA0_PD_MASK;
+  Data = 0x0 << SATA3_CTRL_SATA0_PD_OFFSET;
+
+  /* SATA 1 power up */
+  Mask |= SATA3_CTRL_SATA1_PD_MASK;
+  Data |= 0x0 << SATA3_CTRL_SATA1_PD_OFFSET;
+
+  /* SATA SSU enable */
+  Mask |= SATA3_CTRL_SATA1_ENABLE_MASK;
+  Data |= 0x1 << SATA3_CTRL_SATA1_ENABLE_OFFSET;
+
+  /* SATA port 1 enable */
+  Mask |= SATA3_CTRL_SATA_SSU_MASK;
+  Data |= 0x1 << SATA3_CTRL_SATA_SSU_OFFSET;
+  RegSet (SataBase + SATA3_VENDOR_DATA, Data, Mask);
+
+  /* MBUS request size and interface select register */
+  RegSet (SataBase + SATA3_VENDOR_ADDRESS,
+    SATA_MBUS_SIZE_SELECT_REG << SATA3_VENDOR_ADDR_OFSSET,
+      SATA3_VENDOR_ADDR_MASK);
+
+  /* Mbus regret enable */
+  RegSet (SataBase + SATA3_VENDOR_DATA, 0x1 << SATA_MBUS_REGRET_EN_OFFSET,
+    SATA_MBUS_REGRET_EN_MASK);
+}
+
+STATIC
+EFI_STATUS
+ComPhySataCheckPll (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  EFI_STATUS Status = EFI_SUCCESS;
+  UINT32 Data,Mask;
+  IN EFI_PHYSICAL_ADDRESS Addr;
+
+  Addr = SdIpAddr + SD_EXTERNAL_STATUS0_REG;
+  Data = SD_EXTERNAL_STATUS0_PLL_TX_MASK & SD_EXTERNAL_STATUS0_PLL_RX_MASK;
+  Mask = Data;
+  Data =  PollingWithTimeout (Addr, Data, Mask, 15000);
+
+  if (Data != 0) {
+    DEBUG((DEBUG_INFO, "ComPhy: Read from reg = %p - value = 0x%x\n",
+      HpipeAddr + HPIPE_LANE_STATUS0_REG, Data));
+    DEBUG((DEBUG_ERROR, "ComPhy: SD_EXTERNAL_STATUS0_PLL_TX is %d, SD_EXTERNAL_STATUS0_PLL_RX is %d\n",
+      (Data & SD_EXTERNAL_STATUS0_PLL_TX_MASK),
+      (Data & SD_EXTERNAL_STATUS0_PLL_RX_MASK)));
+    Status = EFI_D_ERROR;
+  }
+
+  return Status;
+}
+
+STATIC
+UINTN
+ComPhySataPowerUp (
+  IN UINT32 Lane,
+  IN EFI_PHYSICAL_ADDRESS HpipeBase,
+  IN EFI_PHYSICAL_ADDRESS ComPhyBase,
+  IN UINT8 SataHostId
+  )
+{
+  EFI_STATUS Status;
+  UINT8 *SataDeviceTable;
+  MVHW_NONDISCOVERABLE_DESC *Desc = &mA7k8kNonDiscoverableDescTemplate;
+  EFI_PHYSICAL_ADDRESS HpipeAddr = HPIPE_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS SdIpAddr = SD_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS ComPhyAddr = COMPHY_ADDR(ComPhyBase, Lane);
+
+  SataDeviceTable = (UINT8 *) PcdGetPtr (PcdPciEAhci);
+
+  if (SataDeviceTable == NULL || SataHostId >= PcdGetSize (PcdPciEAhci)) {
+    DEBUG ((DEBUG_ERROR, "ComPhySata: Sata host %d is undefined\n", SataHostId));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (!MVHW_DEV_ENABLED (Sata, SataHostId)) {
+    DEBUG ((DEBUG_ERROR, "ComPhySata: Sata host %d is disabled\n", SataHostId));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  DEBUG ((DEBUG_INFO, "ComPhySata: Initialize SATA PHYs\n"));
+
+  DEBUG((DEBUG_INFO, "ComPhySataPowerUp: stage: MAC configuration - power down ComPhy\n"));
+
+  ComPhySataMacPowerDown (Desc->AhciBaseAddresses[SataHostId]);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: RFU configurations - hard reset ComPhy\n"));
+
+  ComPhySataRFUConfiguration (ComPhyAddr, SdIpAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Comphy configuration\n"));
+
+  ComPhySataPhyConfiguration (HpipeAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Analog paramters from ETP(HW)\n"));
+
+  ComPhySataSetAnalogParameters (HpipeAddr, SdIpAddr);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: ComPhy power up\n"));
+
+  ComPhySataPhyPowerUp (Desc->AhciBaseAddresses[SataHostId]);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Check PLL\n"));
+
+  Status = ComPhySataCheckPll (HpipeAddr, SdIpAddr);
+
+  return Status;
+}
+
+STATIC
+VOID
+ComPhySgmiiRFUConfiguration (
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr,
+  IN UINT32 SgmiiSpeed
+)
+{
+  UINT32 Mask, Data;
+
+  Mask = COMMON_PHY_CFG1_PWR_UP_MASK;
+  Data = 0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET;
+  Mask |= COMMON_PHY_CFG1_PIPE_SELECT_MASK;
+  Data |= 0x0 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET;
+  RegSet (ComPhyAddr + COMMON_PHY_CFG1_REG, Data, Mask);
+
+  /* Select Baud Rate of Comphy And PD_PLL/Tx/Rx */
+  Mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+  Data = 0x0 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK;
+  Mask |= SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK;
+  if (SgmiiSpeed == COMPHY_SPEED_1_25G) {
+    Data |= 0x6 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET;
+    Data |= 0x6 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET;
+  } else {
+    /* 3.125G */
+    Data |= 0x8 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET;
+    Data |= 0x8 << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET;
+  }
+  Mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+  Data |= 0 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+  Data |= 0 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK;
+  Data |= 1 << SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET;
+  RegSet (SdIpAddr + SD_EXTERNAL_CONFIG0_REG, Data, Mask);
+
+  /* Release from hard reset */
+  Mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+  Data = 0x0 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+  Data |= 0x0 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+  Data |= 0x0 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+  RegSet (SdIpAddr + SD_EXTERNAL_CONFIG1_REG, Data, Mask);
+
+  /* Release from hard reset */
+  Mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK;
+  Data = 0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+  Data |= 0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET;
+  RegSet (SdIpAddr+ SD_EXTERNAL_CONFIG1_REG, Data, Mask);
+
+  /* Wait 1ms - until band gap and ref clock ready */
+  MicroSecondDelay (1000);
+  MemoryFence ();
+}
+
+STATIC
+VOID
+ComPhySgmiiPhyConfiguration (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* Set reference clock */
+  Mask = HPIPE_MISC_REFCLK_SEL_MASK;
+  Data = 0x0 << HPIPE_MISC_REFCLK_SEL_OFFSET;
+  RegSet (HpipeAddr + HPIPE_MISC_REG, Data, Mask);
+
+  /* Power and PLL Control */
+  Mask = HPIPE_PWR_PLL_REF_FREQ_MASK;
+  Data = 0x1 << HPIPE_PWR_PLL_REF_FREQ_OFFSET;
+  Mask |= HPIPE_PWR_PLL_PHY_MODE_MASK;
+  Data |= 0x4 << HPIPE_PWR_PLL_PHY_MODE_OFFSET;
+  RegSet (HpipeAddr + HPIPE_PWR_PLL_REG, Data, Mask);
+
+  /* Loopback register */
+  Mask = HPIPE_LOOPBACK_SEL_MASK;
+  Data = 0x1 << HPIPE_LOOPBACK_SEL_OFFSET;
+  RegSet (HpipeAddr + HPIPE_LOOPBACK_REG, Data, Mask);
+
+  /* Rx control 1 */
+  Mask = HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK;
+  Data = 0x1 << HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET;
+  Mask |= HPIPE_RX_CONTROL_1_CLK8T_EN_MASK;
+  Data |= 0x0 << HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET;
+  RegSet (HpipeAddr + HPIPE_RX_CONTROL_1_REG, Data, Mask);
+
+  /* DTL Control */
+  Mask = HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK;
+  Data = 0x0 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET;
+  RegSet (HpipeAddr + HPIPE_PWR_CTR_DTL_REG, Data, Mask);
+}
+
+STATIC
+EFI_STATUS
+ComPhyEthCommonRFUPowerUp (
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  EFI_STATUS Status = EFI_SUCCESS;
+  UINT32 Mask, Data;
+  EFI_PHYSICAL_ADDRESS Addr;
+
+  /* SerDes External Configuration */
+  Mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK;
+  Data = 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK;
+  Data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK;
+  Data |= 0x1 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET;
+  RegSet (SdIpAddr + SD_EXTERNAL_CONFIG0_REG, Data, Mask);
+
+  /* Check PLL rx & tx ready */
+  Addr = SdIpAddr + SD_EXTERNAL_STATUS0_REG;
+  Data = SD_EXTERNAL_STATUS0_PLL_RX_MASK | SD_EXTERNAL_STATUS0_PLL_TX_MASK;
+  Mask = Data;
+  Data = PollingWithTimeout (Addr, Data, Mask, 15000);
+  if (Data != 0) {
+    DEBUG((DEBUG_ERROR, "ComPhy: Read from reg = %p - value = 0x%x\n",
+      SdIpAddr + SD_EXTERNAL_STATUS0_REG, Data));
+    DEBUG((DEBUG_ERROR, "ComPhy: SD_EXTERNAL_STATUS0_PLL_RX is %d, SD_EXTERNAL_STATUS0_PLL_TX is %d\n",
+      (Data & SD_EXTERNAL_STATUS0_PLL_RX_MASK),
+      (Data & SD_EXTERNAL_STATUS0_PLL_TX_MASK)));
+    Status = EFI_D_ERROR;
+  }
+
+  /* RX init */
+  Mask = SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+  Data = 0x1 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+  RegSet (SdIpAddr + SD_EXTERNAL_CONFIG1_REG, Data, Mask);
+
+  /* Check that RX init done */
+  Addr = SdIpAddr + SD_EXTERNAL_STATUS0_REG;
+  Data = SD_EXTERNAL_STATUS0_RX_INIT_MASK;
+  Mask = Data;
+  Data = PollingWithTimeout (Addr, Data, Mask, 100);
+  if (Data != 0) {
+    DEBUG((DEBUG_ERROR, "ComPhy: Read from reg = %p - value = 0x%x\n",
+      SdIpAddr + SD_EXTERNAL_STATUS0_REG, Data));
+    DEBUG((DEBUG_ERROR, "ComPhy: SD_EXTERNAL_STATUS0_RX_INIT is 0\n"));
+    Status = EFI_D_ERROR;
+  }
+  Mask =  SD_EXTERNAL_CONFIG1_RX_INIT_MASK;
+  Data = 0x0 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET;
+  Mask |= SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+  Data |= 0x1 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET;
+  RegSet (SdIpAddr + SD_EXTERNAL_CONFIG1_REG, Data, Mask);
+
+  return Status;
+}
+
+STATIC
+UINTN
+ComPhySgmiiPowerUp (
+  IN UINT32 Lane,
+  IN UINT32 SgmiiSpeed,
+  IN EFI_PHYSICAL_ADDRESS HpipeBase,
+  IN EFI_PHYSICAL_ADDRESS ComPhyBase
+  )
+{
+  EFI_STATUS Status = EFI_SUCCESS;
+  EFI_PHYSICAL_ADDRESS HpipeAddr = HPIPE_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS SdIpAddr = SD_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS ComPhyAddr = COMPHY_ADDR(ComPhyBase, Lane);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: RFU configurations - hard reset ComPhy\n"));
+
+  ComPhySgmiiRFUConfiguration (ComPhyAddr, SdIpAddr, SgmiiSpeed);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: ComPhy configuration\n"));
+
+  ComPhySgmiiPhyConfiguration (HpipeAddr);
+
+  /* Set analog paramters from ETP(HW) - for now use the default data */
+  DEBUG((DEBUG_INFO, "ComPhy: stage: Analog paramters from ETP(HW)\n"));
+
+  RegSet (HpipeAddr + HPIPE_G1_SET0_REG,
+    0x1 << HPIPE_GX_SET0_TX_EMPH1_OFFSET, HPIPE_GX_SET0_TX_EMPH1_MASK);
+
+  DEBUG((DEBUG_INFO, "ComPhy: stage: RFU configurations - Power Up PLL,Tx,Rx\n"));
+
+  Status = ComPhyEthCommonRFUPowerUp (SdIpAddr);
+
+  return Status;
+}
+
+STATIC
+VOID
+ComPhySfiRFUConfiguration (
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  UINT32 Mask, Data;
+
+  MmioAndThenOr32 (
+          ComPhyAddr + COMMON_PHY_CFG1_REG,
+          ~(COMMON_PHY_CFG1_PWR_UP_MASK | COMMON_PHY_CFG1_PIPE_SELECT_MASK),
+          COMMON_PHY_CFG1_PWR_UP_MASK
+          );
+
+  /* Select Baud Rate of Comphy And PD_PLL/Tx/Rx */
+  Mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK |
+         SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK;
+  Data = (0xe << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET) |
+         (0xe << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET);
+  MmioAndThenOr32 (SdIpAddr + SD_EXTERNAL_CONFIG0_REG, ~Mask, Data);
+
+  /* Release from hard reset */
+  Mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK |
+         SD_EXTERNAL_CONFIG1_RESET_CORE_MASK |
+         SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+  Data = SD_EXTERNAL_CONFIG1_RESET_IN_MASK |
+         SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+  MmioAndThenOr32 (SdIpAddr + SD_EXTERNAL_CONFIG1_REG, ~Mask, Data);
+
+  /* Wait 1ms - until band gap and ref clock are ready */
+  MicroSecondDelay (1000);
+  MemoryFence ();
+}
+
+STATIC
+VOID
+ComPhySfiPhyConfiguration (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr,
+  IN UINT32 SfiSpeed
+)
+{
+  UINT32 Mask, Data;
+
+  /* Set reference clock */
+  Mask = HPIPE_MISC_ICP_FORCE_MASK | HPIPE_MISC_REFCLK_SEL_MASK;
+  Data = (SfiSpeed == COMPHY_SPEED_5_15625G) ?
+    (0x0 << HPIPE_MISC_ICP_FORCE_OFFSET) : (0x1 << HPIPE_MISC_ICP_FORCE_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_MISC_REG, ~Mask, Data);
+
+  /* Power and PLL Control */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_PWR_PLL_REG,
+          ~(HPIPE_PWR_PLL_REF_FREQ_MASK | HPIPE_PWR_PLL_PHY_MODE_MASK),
+          0x1 | (0x4 << HPIPE_PWR_PLL_PHY_MODE_OFFSET)
+          );
+
+  /* Loopback register */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_LOOPBACK_REG,
+          ~HPIPE_LOOPBACK_SEL_MASK,
+          0x1 << HPIPE_LOOPBACK_SEL_OFFSET
+          );
+
+  /* Rx control 1 */
+  MmioOr32 (
+          HpipeAddr + HPIPE_RX_CONTROL_1_REG,
+          HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK | HPIPE_RX_CONTROL_1_CLK8T_EN_MASK
+          );
+
+  /* DTL Control */
+  MmioOr32 (HpipeAddr + HPIPE_PWR_CTR_DTL_REG, HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK);
+
+  /* Transmitter/Receiver Speed Divider Force */
+  if (SfiSpeed == COMPHY_SPEED_5_15625G) {
+    Mask = HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_MASK |
+           HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_MASK |
+           HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_MASK |
+           HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_MASK;
+    Data = (1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET) |
+           (1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET) |
+           (1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET) |
+           (1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET);
+    MmioAndThenOr32 (HpipeAddr + HPIPE_SPD_DIV_FORCE_REG, ~Mask, Data);
+  } else {
+    MmioOr32 (HpipeAddr + HPIPE_SPD_DIV_FORCE_REG, HPIPE_TXDIGCK_DIV_FORCE_MASK);
+  }
+}
+
+STATIC
+VOID
+ComPhySfiSetAnalogParameters (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr,
+  IN UINT32 SfiSpeed
+)
+{
+  UINT32 Mask, Data;
+
+  /* SERDES External Configuration 2 */
+  MmioOr32 (SdIpAddr + SD_EXTERNAL_CONFIG2_REG, SD_EXTERNAL_CONFIG2_PIN_DFE_EN_MASK);
+
+  /* DFE Resolution control */
+  MmioOr32 (HpipeAddr + HPIPE_DFE_REG0, HPIPE_DFE_RES_FORCE_MASK);
+
+  /* Generation 1 setting_0 */
+  if (SfiSpeed == COMPHY_SPEED_5_15625G) {
+    Mask = HPIPE_GX_SET0_TX_EMPH1_MASK;
+    Data = 0x6 << HPIPE_GX_SET0_TX_EMPH1_OFFSET;
+  } else {
+    Mask = HPIPE_GX_SET0_TX_AMP_MASK | HPIPE_GX_SET0_TX_EMPH1_MASK;
+    Data = (0x1c << HPIPE_GX_SET0_TX_AMP_OFFSET) | (0xe << HPIPE_GX_SET0_TX_EMPH1_OFFSET);
+  }
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET0_REG, ~Mask, Data);
+
+  /* Generation 1 setting 2 */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_GX_SET2_REG,
+          ~HPIPE_GX_SET2_TX_EMPH0_MASK,
+          HPIPE_GX_SET2_TX_EMPH0_EN_MASK
+          );
+
+  /* Transmitter Slew Rate Control register */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_TX_REG1_REG,
+          ~(HPIPE_TX_REG1_TX_EMPH_RES_MASK | HPIPE_TX_REG1_SLC_EN_MASK),
+          (0x3 << HPIPE_TX_REG1_TX_EMPH_RES_OFFSET) | (0x3f << HPIPE_TX_REG1_SLC_EN_OFFSET)
+          );
+
+  /* Impedance Calibration Control register */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_CAL_REG1_REG,
+          ~(HPIPE_CAL_REG_1_EXT_TXIMP_MASK | HPIPE_CAL_REG_1_EXT_TXIMP_EN_MASK),
+          (0xe << HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET) | HPIPE_CAL_REG_1_EXT_TXIMP_EN_MASK
+          );
+
+  /* Generation 1 setting 5 */
+  MmioAnd32 (HpipeAddr + HPIPE_G1_SET5_REG, ~HPIPE_GX_SET5_ICP_MASK);
+
+  /* Generation 1 setting 1 */
+  if (SfiSpeed == COMPHY_SPEED_5_15625G) {
+    Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK | HPIPE_GX_SET1_RX_SELMUPP_MASK;
+    Data = 0x1 | (0x1 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET);
+  } else {
+    Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK |
+           HPIPE_GX_SET1_RX_SELMUPP_MASK |
+           HPIPE_GX_SET1_RX_SELMUFI_MASK |
+           HPIPE_GX_SET1_RX_SELMUFF_MASK |
+           HPIPE_GX_SET1_RX_DIGCK_DIV_MASK;
+    Data = 0x2 |
+           (0x2 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET) |
+           (0x1 << HPIPE_GX_SET1_RX_SELMUFF_OFFSET) |
+           (0x3 << HPIPE_GX_SET1_RX_DIGCK_DIV_OFFSET);
+  }
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET1_REG, ~Mask, Data);
+  MmioOr32 (HpipeAddr + HPIPE_G1_SET1_REG, HPIPE_GX_SET1_RX_DFE_EN_MASK);
+
+  /* DFE F3-F5 Coefficient Control */
+  MmioAnd32 (
+          HpipeAddr + HPIPE_DFE_F3_F5_REG,
+          ~(HPIPE_DFE_F3_F5_DFE_EN_MASK | HPIPE_DFE_F3_F5_DFE_CTRL_MASK)
+          );
+
+  /* Configure Generation 1 setting 4 (DFE) */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G1_SET4_REG,
+          ~HPIPE_GX_SET4_DFE_RES_MASK,
+          0x1 << HPIPE_GX_SET4_DFE_RES_OFFSET
+          );
+
+  /* Generation 1 setting 3 */
+  MmioOr32 (HpipeAddr + HPIPE_G1_SET3_REG, HPIPE_GX_SET3_FBCK_SEL_MASK);
+
+  if (SfiSpeed == COMPHY_SPEED_5_15625G) {
+    /* Force FFE (Feed Forward Equalization) to 5G */
+    Mask = HPIPE_GX_SET3_FFE_CAP_SEL_MASK |
+           HPIPE_GX_SET3_FFE_RES_SEL_MASK |
+           HPIPE_GX_SET3_FFE_SETTING_FORCE_MASK;
+    Data = 0xf | (0x4 << HPIPE_GX_SET3_FFE_RES_SEL_OFFSET) | HPIPE_GX_SET3_FFE_SETTING_FORCE_MASK;
+    MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET3_REG, ~Mask, Data);
+  }
+
+  /* Configure RX training timer */
+  MmioAndThenOr32 (HpipeAddr + HPIPE_TX_TRAIN_CTRL_5_REG, ~HPIPE_RX_TRAIN_TIMER_MASK, 0x13);
+
+  /* Enable TX train peak to peak hold */
+  MmioOr32 (HpipeAddr + HPIPE_TX_TRAIN_CTRL_0_REG, HPIPE_TX_TRAIN_P2P_HOLD_MASK);
+
+  /* Configure TX preset index */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_TX_PRESET_INDEX_REG,
+          ~HPIPE_TX_PRESET_INDEX_MASK,
+          0x2 << HPIPE_TX_PRESET_INDEX_OFFSET
+          );
+
+  /* Disable pattern lock lost timeout */
+  MmioAnd32 (HpipeAddr + HPIPE_FRAME_DETECT_CTRL_3_REG, ~HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_MASK);
+
+  /* Configure TX training pattern and TX training 16bit auto */
+  MmioOr32 (
+          HpipeAddr + HPIPE_TX_TRAIN_REG,
+          HPIPE_TX_TRAIN_16BIT_AUTO_EN_MASK | HPIPE_TX_TRAIN_PAT_SEL_MASK
+          );
+
+  /* Configure training pattern number */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_FRAME_DETECT_CTRL_0_REG,
+          ~HPIPE_TRAIN_PAT_NUM_MASK,
+          0x88 << HPIPE_TRAIN_PAT_NUM_OFFSET
+          );
+
+  /* Configure differential manchester encoder to ethernet mode */
+  MmioOr32 (HpipeAddr + HPIPE_DME_REG, HPIPE_DME_ETHERNET_MODE_MASK);
+
+  /* Configure VDD Continuous Calibration */
+  MmioOr32 (HpipeAddr + HPIPE_VDD_CAL_0_REG, HPIPE_CAL_VDD_CONT_MODE_MASK);
+
+  /* Configure sampler gain */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_RX_SAMPLER_OS_GAIN_MASK,
+          0x3 << HPIPE_RX_SAMPLER_OS_GAIN_OFFSET
+          );
+
+  /* Trigger sampler enable pulse (by toggling the bit) */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_SAMPLER_MASK,
+          0x1 << HPIPE_SAMPLER_OFFSET
+          );
+  MmioAnd32 (
+          HpipeAddr + HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG,
+          ~HPIPE_SAMPLER_MASK
+          );
+
+  /* VDD calibration control */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_VDD_CAL_CTRL_REG,
+          ~HPIPE_EXT_SELLV_RXSAMPL_MASK,
+          0x1a << HPIPE_EXT_SELLV_RXSAMPL_OFFSET
+          );
+}
+
+STATIC
+EFI_STATUS
+ComPhySfiPowerUp (
+  IN UINT32 Lane,
+  IN EFI_PHYSICAL_ADDRESS HpipeBase,
+  IN EFI_PHYSICAL_ADDRESS ComPhyBase,
+  IN UINT32 SfiSpeed
+  )
+{
+  EFI_STATUS Status;
+  EFI_PHYSICAL_ADDRESS HpipeAddr = HPIPE_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS SdIpAddr = SD_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS ComPhyAddr = COMPHY_ADDR(ComPhyBase, Lane);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: RFU configurations - hard reset ComPhy\n"));
+
+  ComPhySfiRFUConfiguration (ComPhyAddr, SdIpAddr);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: ComPhy configuration\n"));
+
+  ComPhySfiPhyConfiguration (HpipeAddr, SfiSpeed);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: Set analog paramters\n"));
+
+  ComPhySfiSetAnalogParameters (HpipeAddr, SdIpAddr, SfiSpeed);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: RFU configurations - Power Up PLL,Tx,Rx\n"));
+
+  Status = ComPhyEthCommonRFUPowerUp (SdIpAddr);
+
+  return Status;
+}
+
+STATIC
+EFI_STATUS
+ComPhyRxauiRFUConfiguration (
+  IN UINT32 Lane,
+  IN EFI_PHYSICAL_ADDRESS ComPhyAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  UINT32 Mask, Data;
+
+  MmioAndThenOr32 (
+          ComPhyAddr + COMMON_PHY_CFG1_REG,
+          ~(COMMON_PHY_CFG1_PWR_UP_MASK | COMMON_PHY_CFG1_PIPE_SELECT_MASK),
+          COMMON_PHY_CFG1_PWR_UP_MASK
+          );
+
+  switch (Lane) {
+  case 2:
+  case 4:
+    MmioOr32 (ComPhyAddr + COMMON_PHY_SD_CTRL1, COMMON_PHY_SD_CTRL1_RXAUI0_MASK);
+  case 3:
+  case 5:
+    MmioOr32 (ComPhyAddr + COMMON_PHY_SD_CTRL1, COMMON_PHY_SD_CTRL1_RXAUI1_MASK);
+    break;
+  default:
+    DEBUG ((DEBUG_ERROR, "RXAUI used on invalid lane %d\n", Lane));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  /* Select Baud Rate of Comphy And PD_PLL/Tx/Rx */
+  Mask = SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK |
+         SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK |
+         SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK |
+         SD_EXTERNAL_CONFIG0_MEDIA_MODE_MASK;
+  Data = (0xb << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET) |
+         (0xb << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET) |
+         (0x1 << SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET);
+  MmioAndThenOr32 (SdIpAddr + SD_EXTERNAL_CONFIG0_REG, ~Mask, Data);
+
+  /* Release from hard reset */
+  Mask = SD_EXTERNAL_CONFIG1_RESET_IN_MASK |
+         SD_EXTERNAL_CONFIG1_RESET_CORE_MASK |
+         SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK;
+  Data = SD_EXTERNAL_CONFIG1_RESET_IN_MASK |
+         SD_EXTERNAL_CONFIG1_RESET_CORE_MASK;
+  MmioAndThenOr32 (SdIpAddr + SD_EXTERNAL_CONFIG1_REG, ~Mask, Data);
+
+  /* Wait 1ms - until band gap and ref clock are ready */
+  MicroSecondDelay (1000);
+  MemoryFence ();
+
+  return EFI_SUCCESS;
+}
+
+STATIC
+VOID
+ComPhyRxauiPhyConfiguration (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr
+)
+{
+  /* Set reference clock */
+  MmioAnd32 (HpipeAddr + HPIPE_MISC_REG, ~HPIPE_MISC_REFCLK_SEL_MASK);
+
+  /* Power and PLL Control */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_PWR_PLL_REG,
+          ~(HPIPE_PWR_PLL_REF_FREQ_MASK | HPIPE_PWR_PLL_PHY_MODE_MASK),
+          0x1 | (0x4 << HPIPE_PWR_PLL_PHY_MODE_OFFSET)
+          );
+
+  /* Loopback register */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_LOOPBACK_REG,
+          ~HPIPE_LOOPBACK_SEL_MASK,
+          0x1 << HPIPE_LOOPBACK_SEL_OFFSET
+          );
+
+  /* Rx control 1 */
+  MmioOr32 (
+          HpipeAddr + HPIPE_RX_CONTROL_1_REG,
+          HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK | HPIPE_RX_CONTROL_1_CLK8T_EN_MASK
+          );
+
+  /* DTL Control */
+  MmioAnd32 (HpipeAddr + HPIPE_PWR_CTR_DTL_REG, ~HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK);
+}
+
+STATIC
+VOID
+ComPhyRxauiSetAnalogParameters (
+  IN EFI_PHYSICAL_ADDRESS HpipeAddr,
+  IN EFI_PHYSICAL_ADDRESS SdIpAddr
+)
+{
+  UINT32 Mask, Data;
+
+  /* SERDES External Configuration 2 */
+  MmioOr32 (SdIpAddr + SD_EXTERNAL_CONFIG2_REG, SD_EXTERNAL_CONFIG2_PIN_DFE_EN_MASK);
+
+  /* DFE Resolution control */
+  MmioOr32 (HpipeAddr + HPIPE_DFE_REG0, HPIPE_DFE_RES_FORCE_MASK);
+
+  /* Generation 1 setting_0 */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G1_SET0_REG,
+          ~HPIPE_GX_SET0_TX_EMPH1_MASK,
+          0xe << HPIPE_GX_SET0_TX_EMPH1_OFFSET
+          );
+
+  /* Generation 1 setting 1 */
+  Mask = HPIPE_GX_SET1_RX_SELMUPI_MASK |
+         HPIPE_GX_SET1_RX_SELMUPP_MASK |
+         HPIPE_GX_SET1_RX_DFE_EN_MASK;
+  Data = 0x1 |
+         (0x1 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET) |
+         (0x1 << HPIPE_GX_SET1_RX_DFE_EN_OFFSET);
+  MmioAndThenOr32 (HpipeAddr + HPIPE_G1_SET1_REG, ~Mask, Data);
+
+  /* DFE F3-F5 Coefficient Control */
+  MmioAnd32 (
+          HpipeAddr + HPIPE_DFE_F3_F5_REG,
+          ~(HPIPE_DFE_F3_F5_DFE_EN_MASK | HPIPE_DFE_F3_F5_DFE_CTRL_MASK)
+          );
+
+  /* Configure Generation 1 setting 4 (DFE) */
+  MmioAndThenOr32 (
+          HpipeAddr + HPIPE_G1_SET4_REG,
+          ~HPIPE_GX_SET4_DFE_RES_MASK,
+          0x1 << HPIPE_GX_SET4_DFE_RES_OFFSET
+          );
+
+  /* Generation 1 setting 3 */
+  MmioOr32 (HpipeAddr + HPIPE_G1_SET3_REG, HPIPE_GX_SET3_FBCK_SEL_MASK);
+}
+
+STATIC
+EFI_STATUS
+ComPhyRxauiPowerUp (
+  IN UINT32 Lane,
+  IN EFI_PHYSICAL_ADDRESS HpipeBase,
+  IN EFI_PHYSICAL_ADDRESS ComPhyBase
+  )
+{
+  EFI_STATUS Status;
+  EFI_PHYSICAL_ADDRESS HpipeAddr = HPIPE_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS SdIpAddr = SD_ADDR(HpipeBase, Lane);
+  EFI_PHYSICAL_ADDRESS ComPhyAddr = COMPHY_ADDR(ComPhyBase, Lane);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: RFU configurations - hard reset ComPhy\n"));
+
+  Status = ComPhyRxauiRFUConfiguration (Lane, ComPhyAddr, SdIpAddr);
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: ComPhy configuration\n"));
+
+  ComPhyRxauiPhyConfiguration (HpipeAddr);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: Set analog paramters\n"));
+
+  ComPhyRxauiSetAnalogParameters (HpipeAddr, SdIpAddr);
+
+  DEBUG ((DEBUG_INFO, "ComPhy: stage: RFU configurations - Power Up PLL,Tx,Rx\n"));
+
+  Status = ComPhyEthCommonRFUPowerUp (SdIpAddr);
+
+  return Status;
+}
+
+STATIC
+VOID
+ComPhyMuxCp110 (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg,
+  IN COMPHY_MAP *SerdesMap
+  )
+{
+  EFI_PHYSICAL_ADDRESS ComPhyBaseAddr;
+  COMPHY_MAP ComPhyMapPipeData[MAX_LANE_OPTIONS];
+  COMPHY_MAP ComPhyMapPhyData[MAX_LANE_OPTIONS];
+  UINT32 Lane, ComPhyMaxCount;
+
+  ComPhyMaxCount = PtrChipCfg->LanesCount;
+  ComPhyBaseAddr = PtrChipCfg->ComPhyBaseAddr;
+
+  /*
+   * Copy the SerDes map configuration for PIPE map and PHY map.
+   * The ComPhyMuxInit modifies the Type of the Lane if the Type is not valid.
+   * Because we have 2 selectors, run the ComPhyMuxInit twice and after
+   * that, update the original SerdesMap.
+   */
+  for (Lane = 0; Lane < ComPhyMaxCount; Lane++) {
+    ComPhyMapPipeData[Lane].Type = SerdesMap[Lane].Type;
+    ComPhyMapPipeData[Lane].Speed = SerdesMap[Lane].Speed;
+    ComPhyMapPhyData[Lane].Type = SerdesMap[Lane].Type;
+    ComPhyMapPhyData[Lane].Speed = SerdesMap[Lane].Speed;
+  }
+  PtrChipCfg->MuxData = Cp110ComPhyMuxData;
+  ComPhyMuxInit(PtrChipCfg, ComPhyMapPhyData, ComPhyBaseAddr +
+    COMMON_SELECTOR_PHY_OFFSET);
+
+  PtrChipCfg->MuxData = Cp110ComPhyPipeMuxData;
+  ComPhyMuxInit(PtrChipCfg, ComPhyMapPipeData, ComPhyBaseAddr +
+    COMMON_SELECTOR_PIPE_OFFSET);
+
+  /* Fix the Type after check the PHY and PIPE configuration */
+  for (Lane = 0; Lane < ComPhyMaxCount; Lane++)
+    if ((ComPhyMapPipeData[Lane].Type == COMPHY_TYPE_UNCONNECTED) &&
+        (ComPhyMapPhyData[Lane].Type == COMPHY_TYPE_UNCONNECTED))
+      SerdesMap[Lane].Type = COMPHY_TYPE_UNCONNECTED;
+}
+
+VOID
+ComPhyCp110Init (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg
+  )
+{
+  EFI_STATUS Status;
+  COMPHY_MAP *PtrComPhyMap, *SerdesMap;
+  EFI_PHYSICAL_ADDRESS ComPhyBaseAddr, HpipeBaseAddr;
+  UINT32 ComPhyMaxCount, Lane;
+  UINT32 PcieBy4 = 1; // Indicating if first 4 lanes set to PCIE
+
+  ComPhyMaxCount = PtrChipCfg->LanesCount;
+  ComPhyBaseAddr = PtrChipCfg->ComPhyBaseAddr;
+  HpipeBaseAddr = PtrChipCfg->Hpipe3BaseAddr;
+  SerdesMap = PtrChipCfg->MapData;
+
+  /* Config Comphy mux configuration */
+  ComPhyMuxCp110(PtrChipCfg, SerdesMap);
+
+  /* Check if the first 4 Lanes configured as By-4 */
+  for (Lane = 0, PtrComPhyMap = SerdesMap; Lane < 4; Lane++, PtrComPhyMap++) {
+    if (PtrComPhyMap->Type != COMPHY_TYPE_PCIE0) {
+      PcieBy4 = 0;
+      break;
+    }
+  }
+
+  for (Lane = 0, PtrComPhyMap = SerdesMap; Lane < ComPhyMaxCount;
+       Lane++, PtrComPhyMap++) {
+    DEBUG((DEBUG_INFO, "ComPhy: Initialize serdes number %d\n", Lane));
+    DEBUG((DEBUG_INFO, "ComPhy: Serdes Type = 0x%x\n", PtrComPhyMap->Type));
+    switch (PtrComPhyMap->Type) {
+    case COMPHY_TYPE_UNCONNECTED:
+      continue;
+      break;
+    case COMPHY_TYPE_PCIE0:
+    case COMPHY_TYPE_PCIE1:
+    case COMPHY_TYPE_PCIE2:
+    case COMPHY_TYPE_PCIE3:
+      Status = ComPhyPciePowerUp(Lane, PcieBy4, HpipeBaseAddr, ComPhyBaseAddr);
+      break;
+    case COMPHY_TYPE_SATA0:
+    case COMPHY_TYPE_SATA1:
+      Status = ComPhySataPowerUp (Lane, HpipeBaseAddr, ComPhyBaseAddr, MVHW_CP0_AHCI0_ID);
+      break;
+    case COMPHY_TYPE_SATA2:
+    case COMPHY_TYPE_SATA3:
+      Status = ComPhySataPowerUp (Lane, HpipeBaseAddr, ComPhyBaseAddr, MVHW_CP1_AHCI0_ID);
+      break;
+    case COMPHY_TYPE_USB3_HOST0:
+    case COMPHY_TYPE_USB3_HOST1:
+      Status = ComphyUsb3PowerUp(Lane, HpipeBaseAddr, ComPhyBaseAddr);
+      break;
+    case COMPHY_TYPE_SGMII0:
+    case COMPHY_TYPE_SGMII1:
+    case COMPHY_TYPE_SGMII2:
+    case COMPHY_TYPE_SGMII3:
+      Status = ComPhySgmiiPowerUp(Lane, PtrComPhyMap->Speed, HpipeBaseAddr,
+        ComPhyBaseAddr);
+      break;
+    case COMPHY_TYPE_SFI:
+      Status = ComPhySfiPowerUp(Lane, HpipeBaseAddr, ComPhyBaseAddr, PtrComPhyMap->Speed);
+      break;
+    case COMPHY_TYPE_RXAUI0:
+    case COMPHY_TYPE_RXAUI1:
+      Status = ComPhyRxauiPowerUp(Lane, HpipeBaseAddr, ComPhyBaseAddr);
+      break;
+    default:
+      DEBUG((DEBUG_ERROR, "Unknown SerDes Type, skip initialize SerDes %d\n",
+        Lane));
+      Status = EFI_INVALID_PARAMETER;
+      ASSERT (FALSE);
+      break;
+    }
+    if (EFI_ERROR(Status)) {
+      DEBUG ((DEBUG_ERROR, "Failed to initialize Lane %d\n with Status = 0x%x", Lane, Status));
+      PtrComPhyMap->Type = COMPHY_TYPE_UNCONNECTED;
+    }
+  }
+}
diff --git a/Platform/Marvell/Library/ComPhyLib/ComPhyLib.c b/Platform/Marvell/Library/ComPhyLib/ComPhyLib.c
new file mode 100644
index 0000000000..3eb5d9f6a9
--- /dev/null
+++ b/Platform/Marvell/Library/ComPhyLib/ComPhyLib.c
@@ -0,0 +1,321 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#include "ComPhyLib.h"
+#include <Library/MvComPhyLib.h>
+#include <Library/MvHwDescLib.h>
+
+DECLARE_A7K8K_COMPHY_TEMPLATE;
+
+CHAR16 * TypeStringTable [] = {L"unconnected", L"PCIE0", L"PCIE1", L"PCIE2",
+                           L"PCIE3", L"SATA0", L"SATA1", L"SATA2", L"SATA3",
+                           L"SGMII0", L"SGMII1", L"SGMII2", L"SGMII3",
+                           L"QSGMII", L"USB3_HOST0", L"USB3_HOST1",
+                           L"USB3_DEVICE", L"XAUI0", L"XAUI1", L"XAUI2",
+                           L"XAUI3", L"RXAUI0", L"RXAUI1", L"SFI"};
+
+CHAR16 * SpeedStringTable [] = {L"-", L"1.25 Gbps", L"1.5 Gbps", L"2.5 Gbps",
+                                L"3.0 Gbps", L"3.125 Gbps", L"5 Gbps", L"5.156 Gbps",
+                                L"6 Gbps", L"6.25 Gbps", L"10.31 Gbps"};
+
+CHIP_COMPHY_CONFIG ChipCfgTbl[] = {
+  {
+    .ChipType = MvComPhyTypeCp110,
+    .Init = ComPhyCp110Init
+  },
+};
+
+VOID
+RegSet (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Data,
+  IN UINT32 Mask
+  )
+{
+  DEBUG((DEBUG_INFO, "Write to address = %10x, data = %10x (mask = %10x)"
+    "- ", Addr, Data, Mask));
+  DEBUG((DEBUG_INFO, "old value = %10x ==> ", MmioRead32 (Addr)));
+  RegSetSilent (Addr, Data, Mask);
+  DEBUG((DEBUG_INFO, "new value %10x\n", MmioRead32 (Addr)));
+}
+
+VOID
+RegSetSilent (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Data,
+  IN UINT32 Mask
+  )
+{
+  UINT32 RegData;
+
+  RegData = MmioRead32 (Addr);
+  RegData &= ~Mask;
+  RegData |= Data;
+  MmioWrite32 (Addr, RegData);
+}
+
+VOID
+RegSet16 (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT16 Data,
+  IN UINT16 Mask
+  )
+{
+  DEBUG((DEBUG_INFO, "Write to address = %#010lx, Data = %#06x (mask = %#06x)"
+    "- ", Addr, Data, Mask));
+  DEBUG((DEBUG_INFO, "old value = %#06x ==> ", MmioRead16 (Addr)));
+  RegSetSilent16 (Addr, Data, Mask);
+  DEBUG((DEBUG_INFO, "new value %#06x\n", MmioRead16 (Addr)));
+}
+
+VOID
+RegSetSilent16(
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT16 Data,
+  IN UINT16 Mask
+  )
+{
+  UINT16 RegData;
+  RegData = MmioRead16(Addr);
+  RegData &= ~Mask;
+  RegData |= Data;
+  MmioWrite16 (Addr, RegData);
+}
+
+/* This function returns enum with SerDesType */
+UINT32
+ParseSerdesTypeString (
+  CHAR16* String
+  )
+{
+  UINT32 i;
+
+  if (String == NULL)
+    return COMPHY_TYPE_INVALID;
+
+  for (i = 0; i < COMPHY_TYPE_MAX; i++) {
+    if (StrCmp (String, TypeStringTable[i]) == 0) {
+      return i;
+    }
+  }
+
+  /* PCD string doesn't match any supported SerDes Type */
+  return COMPHY_TYPE_INVALID;
+}
+
+/* This function converts SerDes speed in MHz to enum with SerDesSpeed */
+UINT32
+ParseSerdesSpeed (
+  UINT32 Value
+  )
+{
+  UINT32 i;
+  UINT32 ValueTable [] = {0, 1250, 1500, 2500, 3000, 3125,
+                          5000, 5156, 6000, 6250, 10310};
+
+  for (i = 0; i < COMPHY_SPEED_MAX; i++) {
+    if (Value == ValueTable[i]) {
+      return i;
+    }
+  }
+
+  /* PCD SerDes speed value doesn't match any supported SerDes speed */
+  return COMPHY_SPEED_INVALID;
+}
+
+CHAR16 *
+GetTypeString (
+  UINT32 Type
+  )
+{
+
+  if (Type < 0 || Type > COMPHY_TYPE_MAX) {
+    return L"invalid";
+  }
+
+  return TypeStringTable[Type];
+}
+
+CHAR16 *
+GetSpeedString (
+  UINT32 Speed
+  )
+{
+
+  if (Speed < 0 || Speed > 10) {
+    return L"invalid";
+  }
+
+  return SpeedStringTable[Speed];
+}
+
+VOID
+ComPhyPrint (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg
+  )
+{
+  UINT32 Lane;
+  CHAR16 *SpeedStr, *TypeStr;
+
+  for (Lane = 0; Lane < PtrChipCfg->LanesCount; Lane++) {
+    SpeedStr = GetSpeedString(PtrChipCfg->MapData[Lane].Speed);
+    TypeStr = GetTypeString(PtrChipCfg->MapData[Lane].Type);
+    DEBUG((DEBUG_ERROR, "Comphy-%d: %-13s %-10s\n", Lane, TypeStr, SpeedStr));
+  }
+
+  DEBUG((DEBUG_ERROR, "\n"));
+}
+
+EFI_STATUS
+GetChipComPhyInit (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg
+  )
+{
+  UINTN i, TblSize;
+
+
+  TblSize = sizeof(ChipCfgTbl) / sizeof(ChipCfgTbl[0]);
+
+  for (i = 0; i < TblSize ; i++) {
+    if (PtrChipCfg->ChipType == ChipCfgTbl[i].ChipType) {
+      PtrChipCfg->Init = ChipCfgTbl[i].Init;
+      return EFI_SUCCESS;
+    }
+  }
+
+  return EFI_D_ERROR;
+}
+
+STATIC
+VOID
+InitComPhyConfig (
+  IN  OUT  CHIP_COMPHY_CONFIG *ChipConfig,
+  IN  OUT  PCD_LANE_MAP       *LaneData,
+  IN       UINT8               Id
+  )
+{
+  MVHW_COMPHY_DESC *Desc = &mA7k8kComPhyDescTemplate;
+
+  ChipConfig->ChipType = Desc->ComPhyChipType[Id];
+  ChipConfig->ComPhyBaseAddr = Desc->ComPhyBaseAddresses[Id];
+  ChipConfig->Hpipe3BaseAddr = Desc->ComPhyHpipe3BaseAddresses[Id];
+  ChipConfig->LanesCount = Desc->ComPhyLaneCount[Id];
+  ChipConfig->MuxBitCount = Desc->ComPhyMuxBitCount[Id];
+
+  /*
+   * Below macro contains variable name concatenation (used to form PCD's name).
+   */
+  switch (Id) {
+  case 0:
+    GetComPhyPcd (ChipConfig, LaneData, 0);
+    break;
+  case 1:
+    GetComPhyPcd (ChipConfig, LaneData, 1);
+    break;
+  case 2:
+    GetComPhyPcd (ChipConfig, LaneData, 2);
+    break;
+  case 3:
+    GetComPhyPcd (ChipConfig, LaneData, 3);
+    break;
+  }
+}
+
+EFI_STATUS
+MvComPhyInit (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  CHIP_COMPHY_CONFIG ChipConfig[MVHW_MAX_COMPHY_DEVS], *PtrChipCfg;
+  PCD_LANE_MAP LaneData[MVHW_MAX_COMPHY_DEVS];
+  UINT32 Lane, MaxComphyCount;
+  UINT8 *ComPhyDeviceTable, Index;
+
+  /* Obtain table with enabled ComPhy devices */
+  ComPhyDeviceTable = (UINT8 *)PcdGetPtr (PcdComPhyDevices);
+  if (ComPhyDeviceTable == NULL) {
+    DEBUG ((DEBUG_ERROR, "Missing PcdComPhyDevices\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (PcdGetSize (PcdComPhyDevices) > MVHW_MAX_COMPHY_DEVS) {
+    DEBUG ((DEBUG_ERROR, "Wrong PcdComPhyDevices format\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  /* Initialize enabled chips */
+  for (Index = 0; Index < PcdGetSize (PcdComPhyDevices); Index++) {
+    if (!MVHW_DEV_ENABLED (ComPhy, Index)) {
+      DEBUG ((DEBUG_ERROR, "Skip ComPhy chip %d\n", Index));
+      continue;
+    }
+
+    PtrChipCfg = &ChipConfig[Index];
+    InitComPhyConfig(PtrChipCfg, LaneData, Index);
+
+    /* Get the count of the SerDes of the specific chip */
+    MaxComphyCount = PtrChipCfg->LanesCount;
+    for (Lane = 0; Lane < MaxComphyCount; Lane++) {
+      /* Parse PCD with string indicating SerDes Type */
+      PtrChipCfg->MapData[Lane].Type =
+        ParseSerdesTypeString (LaneData[Index].TypeStr[Lane]);
+      PtrChipCfg->MapData[Lane].Speed =
+        ParseSerdesSpeed (LaneData[Index].SpeedValue[Lane]);
+      PtrChipCfg->MapData[Lane].Invert = (UINT32)LaneData[Index].InvFlag[Lane];
+
+      if ((PtrChipCfg->MapData[Lane].Speed == COMPHY_SPEED_INVALID) ||
+          (PtrChipCfg->MapData[Lane].Speed == COMPHY_SPEED_ERROR) ||
+          (PtrChipCfg->MapData[Lane].Type == COMPHY_TYPE_INVALID)) {
+        DEBUG((DEBUG_ERROR, "ComPhy: No valid phy speed or type for lane %d, "
+          "setting lane as unconnected\n", Lane + 1));
+        PtrChipCfg->MapData[Lane].Type = COMPHY_TYPE_UNCONNECTED;
+        PtrChipCfg->MapData[Lane].Speed = COMPHY_SPEED_INVALID;
+      }
+    };
+
+    Status = GetChipComPhyInit (PtrChipCfg);
+    if (EFI_ERROR(Status)) {
+     DEBUG ((DEBUG_ERROR, "ComPhy: Invalid Chip%d type\n", Index));
+     return Status;
+    }
+
+    ComPhyPrint (PtrChipCfg);
+
+    /* PHY power UP sequence */
+    PtrChipCfg->Init (PtrChipCfg);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Platform/Marvell/Library/ComPhyLib/ComPhyLib.h b/Platform/Marvell/Library/ComPhyLib/ComPhyLib.h
new file mode 100644
index 0000000000..38989780ab
--- /dev/null
+++ b/Platform/Marvell/Library/ComPhyLib/ComPhyLib.h
@@ -0,0 +1,643 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#ifndef __COMPHY_H__
+#define __COMPHY_H__
+
+#include <Library/ArmLib.h>
+#include <Library/ArmPlatformLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/MvComPhyLib.h>
+#include <Library/IoLib.h>
+#include <Library/TimerLib.h>
+#include <Library/ParsePcdLib.h>
+
+#define MAX_LANE_OPTIONS          10
+
+/***** Parsing PCD *****/
+#define GET_LANE_TYPE(id)         PcdGetPtr(PcdChip##id##ComPhyTypes)
+#define GET_LANE_SPEED(id)        PcdGetPtr(PcdChip##id##ComPhySpeeds)
+#define GET_LANE_INV(id)          PcdGetPtr(PcdChip##id##ComPhyInvFlags)
+
+#define FillLaneMap(chip_struct, lane_struct, id) { \
+  ParsePcdString((CHAR16 *) GET_LANE_TYPE(id), chip_struct[id].LanesCount, NULL, lane_struct[id].TypeStr);     \
+  ParsePcdString((CHAR16 *) GET_LANE_SPEED(id), chip_struct[id].LanesCount, lane_struct[id].SpeedValue, NULL); \
+  ParsePcdString((CHAR16 *) GET_LANE_INV(id), chip_struct[id].LanesCount, lane_struct[id].InvFlag, NULL);      \
+}
+
+#define GetComPhyPcd(chip_struct, lane_struct, id) {               \
+  FillLaneMap(chip_struct, lane_struct, id);                       \
+}
+
+/***** ComPhy *****/
+#define COMPHY_SPEED_ERROR                           0
+#define COMPHY_SPEED_1_25G                           1
+#define COMPHY_SPEED_1_5G                            2
+#define COMPHY_SPEED_2_5G                            3
+#define COMPHY_SPEED_3G                              4
+#define COMPHY_SPEED_3_125G                          5
+#define COMPHY_SPEED_5G                              6
+#define COMPHY_SPEED_5_15625G                        7
+#define COMPHY_SPEED_6G                              8
+#define COMPHY_SPEED_6_25G                           9
+#define COMPHY_SPEED_10_3125G                        10
+#define COMPHY_SPEED_MAX                             11
+#define COMPHY_SPEED_INVALID                         0xff
+
+#define COMPHY_TYPE_UNCONNECTED                      0
+#define COMPHY_TYPE_PCIE0                            1
+#define COMPHY_TYPE_PCIE1                            2
+#define COMPHY_TYPE_PCIE2                            3
+#define COMPHY_TYPE_PCIE3                            4
+#define COMPHY_TYPE_SATA0                            5
+#define COMPHY_TYPE_SATA1                            6
+#define COMPHY_TYPE_SATA2                            7
+#define COMPHY_TYPE_SATA3                            8
+#define COMPHY_TYPE_SGMII0                           9
+#define COMPHY_TYPE_SGMII1                           10
+#define COMPHY_TYPE_SGMII2                           11
+#define COMPHY_TYPE_SGMII3                           12
+#define COMPHY_TYPE_QSGMII                           13
+#define COMPHY_TYPE_USB3_HOST0                       14
+#define COMPHY_TYPE_USB3_HOST1                       15
+#define COMPHY_TYPE_USB3_DEVICE                      16
+#define COMPHY_TYPE_XAUI0                            17
+#define COMPHY_TYPE_XAUI1                            18
+#define COMPHY_TYPE_XAUI2                            19
+#define COMPHY_TYPE_XAUI3                            20
+#define COMPHY_TYPE_RXAUI0                           21
+#define COMPHY_TYPE_RXAUI1                           22
+#define COMPHY_TYPE_SFI                              23
+#define COMPHY_TYPE_MAX                              24
+#define COMPHY_TYPE_INVALID                          0xff
+
+#define COMPHY_POLARITY_NO_INVERT                    0
+#define COMPHY_POLARITY_TXD_INVERT                   1
+#define COMPHY_POLARITY_RXD_INVERT                   2
+#define COMPHY_POLARITY_ALL_INVERT                   (COMPHY_POLARITY_TXD_INVERT | COMPHY_POLARITY_RXD_INVERT)
+
+/***** SerDes IP registers *****/
+#define SD_EXTERNAL_CONFIG0_REG                   0
+#define SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET      1
+#define SD_EXTERNAL_CONFIG0_SD_PU_PLL_MASK        (1 << SD_EXTERNAL_CONFIG0_SD_PU_PLL_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET  3
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_MASK    (0xf << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_RX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET  7
+#define SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_MASK    (0xf << SD_EXTERNAL_CONFIG0_SD_PHY_GEN_TX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET       11
+#define SD_EXTERNAL_CONFIG0_SD_PU_RX_MASK         (1 << SD_EXTERNAL_CONFIG0_SD_PU_RX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET       12
+#define SD_EXTERNAL_CONFIG0_SD_PU_TX_MASK         (1 << SD_EXTERNAL_CONFIG0_SD_PU_TX_OFFSET)
+#define SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET  14
+#define SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_MASK    (1 << SD_EXTERNAL_CONFIG0_HALF_BUS_MODE_OFFSET)
+#define SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET     15
+#define SD_EXTERNAL_CONFIG0_MEDIA_MODE_MASK       (0x1 << SD_EXTERNAL_CONFIG0_MEDIA_MODE_OFFSET)
+
+#define SD_EXTERNAL_CONFIG1_REG                   0x4
+#define SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET       3
+#define SD_EXTERNAL_CONFIG1_RESET_IN_MASK         (0x1 << SD_EXTERNAL_CONFIG1_RESET_IN_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET        4
+#define SD_EXTERNAL_CONFIG1_RX_INIT_MASK          (0x1 << SD_EXTERNAL_CONFIG1_RX_INIT_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET     5
+#define SD_EXTERNAL_CONFIG1_RESET_CORE_MASK       (0x1 << SD_EXTERNAL_CONFIG1_RESET_CORE_OFFSET)
+#define SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET    6
+#define SD_EXTERNAL_CONFIG1_RF_RESET_IN_MASK      (0x1 << SD_EXTERNAL_CONFIG1_RF_RESET_IN_OFFSET)
+
+#define SD_EXTERNAL_CONFIG2_REG                   0x8
+#define SD_EXTERNAL_CONFIG2_PIN_DFE_EN_OFFSET     4
+#define SD_EXTERNAL_CONFIG2_PIN_DFE_EN_MASK       (0x1 << SD_EXTERNAL_CONFIG2_PIN_DFE_EN_OFFSET)
+#define SD_EXTERNAL_CONFIG2_SSC_ENABLE_OFFSET     7
+#define SD_EXTERNAL_CONFIG2_SSC_ENABLE_MASK       (0x1 << SD_EXTERNAL_CONFIG2_SSC_ENABLE_OFFSET)
+
+#define SD_EXTERNAL_STATUS0_REG                   0x18
+#define SD_EXTERNAL_STATUS0_PLL_TX_OFFSET         2
+#define SD_EXTERNAL_STATUS0_PLL_TX_MASK           (0x1 << SD_EXTERNAL_STATUS0_PLL_TX_OFFSET)
+#define SD_EXTERNAL_STATUS0_PLL_RX_OFFSET         3
+#define SD_EXTERNAL_STATUS0_PLL_RX_MASK           (0x1 << SD_EXTERNAL_STATUS0_PLL_RX_OFFSET)
+#define SD_EXTERNAL_STATUS0_RX_INIT_OFFSET        4
+#define SD_EXTERNAL_STATUS0_RX_INIT_MASK          (0x1 << SD_EXTERNAL_STATUS0_RX_INIT_OFFSET)
+
+/***** HPIPE registers *****/
+#define HPIPE_PWR_PLL_REG                         0x4
+#define HPIPE_PWR_PLL_REF_FREQ_OFFSET             0
+#define HPIPE_PWR_PLL_REF_FREQ_MASK               (0x1f << HPIPE_PWR_PLL_REF_FREQ_OFFSET)
+#define HPIPE_PWR_PLL_PHY_MODE_OFFSET             5
+#define HPIPE_PWR_PLL_PHY_MODE_MASK               (0x7 << HPIPE_PWR_PLL_PHY_MODE_OFFSET)
+
+#define HPIPE_KVCO_CALIB_CTRL_REG                 0x8
+#define HPIPE_KVCO_CALIB_CTRL_MAX_PLL_OFFSET      12
+#define HPIPE_KVCO_CALIB_CTRL_MAX_PLL_MASK        (0x1 << HPIPE_KVCO_CALIB_CTRL_MAX_PLL_OFFSET)
+
+#define HPIPE_CAL_REG1_REG                        0xc
+#define HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET          10
+#define HPIPE_CAL_REG_1_EXT_TXIMP_MASK            (0x1f << HPIPE_CAL_REG_1_EXT_TXIMP_OFFSET)
+#define HPIPE_CAL_REG_1_EXT_TXIMP_EN_OFFSET       15
+#define HPIPE_CAL_REG_1_EXT_TXIMP_EN_MASK         (0x1 << HPIPE_CAL_REG_1_EXT_TXIMP_EN_OFFSET)
+
+#define HPIPE_SQUELCH_FFE_SETTING_REG             0x018
+
+#define HPIPE_DFE_REG0                            0x01C
+#define HPIPE_DFE_RES_FORCE_OFFSET                15
+#define HPIPE_DFE_RES_FORCE_MASK                  (0x1 << HPIPE_DFE_RES_FORCE_OFFSET)
+
+#define HPIPE_DFE_F3_F5_REG                       0x028
+#define HPIPE_DFE_F3_F5_DFE_EN_OFFSET             14
+#define HPIPE_DFE_F3_F5_DFE_EN_MASK               (0x1 << HPIPE_DFE_F3_F5_DFE_EN_OFFSET)
+#define HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET           15
+#define HPIPE_DFE_F3_F5_DFE_CTRL_MASK             (0x1 << HPIPE_DFE_F3_F5_DFE_CTRL_OFFSET)
+
+#define HPIPE_G1_SET0_REG                         0x034
+#define HPIPE_G2_SET0_REG                         0x03c
+#define HPIPE_G3_SET0_REG                         0x044
+#define HPIPE_GX_SET0_TX_AMP_OFFSET               1
+#define HPIPE_GX_SET0_TX_AMP_MASK                 (0x1f << HPIPE_GX_SET0_TX_AMP_OFFSET)
+#define HPIPE_GX_SET0_TX_AMP_ADJ_OFFSET           6
+#define HPIPE_GX_SET0_TX_AMP_ADJ_MASK             (0x1 << HPIPE_GX_SET0_TX_AMP_ADJ_OFFSET)
+#define HPIPE_GX_SET0_TX_EMPH1_OFFSET             7
+#define HPIPE_GX_SET0_TX_EMPH1_MASK               (0xf << HPIPE_GX_SET0_TX_EMPH1_OFFSET)
+#define HPIPE_GX_SET0_TX_EMPH1_EN_OFFSET          11
+#define HPIPE_GX_SET0_TX_EMPH1_EN_MASK            (0x1 << HPIPE_GX_SET0_TX_EMPH1_EN_OFFSET)
+#define HPIPE_GX_SET0_TX_SLEW_RATE_SEL_OFFSET     12
+#define HPIPE_GX_SET0_TX_SLEW_RATE_SEL_MASK       (0x7 << HPIPE_GX_SET0_TX_SLEW_RATE_SEL_OFFSET)
+#define HPIPE_GX_SET0_TX_SLEW_CTRL_EN_OFFSET      15
+#define HPIPE_GX_SET0_TX_SLEW_CTRL_EN_MASK        (0x1 << HPIPE_GX_SET0_TX_SLEW_CTRL_EN_OFFSET)
+
+#define HPIPE_G1_SET1_REG                         0x038
+#define HPIPE_G2_SET1_REG                         0x040
+#define HPIPE_G3_SET1_REG                         0x048
+#define HPIPE_GX_SET1_RX_SELMUPI_OFFSET           0
+#define HPIPE_GX_SET1_RX_SELMUPI_MASK             (0x7 << HPIPE_GX_SET1_RX_SELMUPI_OFFSET)
+#define HPIPE_GX_SET1_RX_SELMUPP_OFFSET           3
+#define HPIPE_GX_SET1_RX_SELMUPP_MASK             (0x7 << HPIPE_GX_SET1_RX_SELMUPP_OFFSET)
+#define HPIPE_GX_SET1_RX_SELMUFI_OFFSET           6
+#define HPIPE_GX_SET1_RX_SELMUFI_MASK             (0x3 << HPIPE_GX_SET1_RX_SELMUFI_OFFSET)
+#define HPIPE_GX_SET1_RX_SELMUFF_OFFSET           8
+#define HPIPE_GX_SET1_RX_SELMUFF_MASK             (0x3 << HPIPE_GX_SET1_RX_SELMUFF_OFFSET)
+#define HPIPE_GX_SET1_RX_DFE_EN_OFFSET            10
+#define HPIPE_GX_SET1_RX_DFE_EN_MASK              (0x1 << HPIPE_GX_SET1_RX_DFE_EN_OFFSET)
+#define HPIPE_GX_SET1_RX_DIGCK_DIV_OFFSET         11
+#define HPIPE_GX_SET1_RX_DIGCK_DIV_MASK           (0x3 << HPIPE_GX_SET1_RX_DIGCK_DIV_OFFSET)
+#define HPIPE_GX_SET1_SAMPLER_INPAIRX2_EN_OFFSET  13
+#define HPIPE_GX_SET1_SAMPLER_INPAIRX2_EN_MASK    (0x1 << HPIPE_GX_SET1_SAMPLER_INPAIRX2_EN_OFFSET)
+
+#define HPIPE_LOOPBACK_REG                        0x08c
+#define HPIPE_LOOPBACK_SEL_OFFSET                 1
+#define HPIPE_LOOPBACK_SEL_MASK                   (0x7 << HPIPE_LOOPBACK_SEL_OFFSET)
+
+#define HPIPE_SYNC_PATTERN_REG                    0x090
+
+#define HPIPE_INTERFACE_REG                       0x94
+#define HPIPE_INTERFACE_GEN_MAX_OFFSET            10
+#define HPIPE_INTERFACE_GEN_MAX_MASK              (0x3 << HPIPE_INTERFACE_GEN_MAX_OFFSET)
+#define HPIPE_INTERFACE_DET_BYPASS_OFFSET         12
+#define HPIPE_INTERFACE_DET_BYPASS_MASK           (0x1 << HPIPE_INTERFACE_DET_BYPASS_OFFSET)
+#define HPIPE_INTERFACE_LINK_TRAIN_OFFSET         14
+#define HPIPE_INTERFACE_LINK_TRAIN_MASK           (0x1 << HPIPE_INTERFACE_LINK_TRAIN_OFFSET)
+
+#define HPIPE_ISOLATE_MODE_REG                    0x98
+#define HPIPE_ISOLATE_MODE_GEN_RX_OFFSET          0
+#define HPIPE_ISOLATE_MODE_GEN_RX_MASK            (0xf << HPIPE_ISOLATE_MODE_GEN_RX_OFFSET)
+#define HPIPE_ISOLATE_MODE_GEN_TX_OFFSET          4
+#define HPIPE_ISOLATE_MODE_GEN_TX_MASK            (0xf << HPIPE_ISOLATE_MODE_GEN_TX_OFFSET)
+
+#define HPIPE_GX_SET2_REG                         0xf4
+#define HPIPE_GX_SET2_TX_EMPH0_OFFSET             0
+#define HPIPE_GX_SET2_TX_EMPH0_MASK               (0xf << HPIPE_GX_SET2_TX_EMPH0_OFFSET)
+#define HPIPE_GX_SET2_TX_EMPH0_EN_OFFSET          4
+#define HPIPE_GX_SET2_TX_EMPH0_EN_MASK            (0x1 << HPIPE_GX_SET2_TX_EMPH0_MASK)
+
+#define HPIPE_VTHIMPCAL_CTRL_REG                  0x104
+
+#define HPIPE_VDD_CAL_CTRL_REG                    0x114
+#define HPIPE_EXT_SELLV_RXSAMPL_OFFSET            5
+#define HPIPE_EXT_SELLV_RXSAMPL_MASK              (0x1f << HPIPE_EXT_SELLV_RXSAMPL_OFFSET)
+
+#define HPIPE_VDD_CAL_0_REG                       0x108
+#define HPIPE_CAL_VDD_CONT_MODE_OFFSET            15
+#define HPIPE_CAL_VDD_CONT_MODE_MASK              (0x1 << HPIPE_CAL_VDD_CONT_MODE_OFFSET)
+
+#define HPIPE_PCIE_REG0                           0x120
+#define HPIPE_PCIE_IDLE_SYNC_OFFSET               12
+#define HPIPE_PCIE_IDLE_SYNC_MASK                 (0x1 << HPIPE_PCIE_IDLE_SYNC_OFFSET)
+#define HPIPE_PCIE_SEL_BITS_OFFSET                13
+#define HPIPE_PCIE_SEL_BITS_MASK                  (0x3 << HPIPE_PCIE_SEL_BITS_OFFSET)
+
+#define HPIPE_LANE_ALIGN_REG                      0x124
+#define HPIPE_LANE_ALIGN_OFF_OFFSET               12
+#define HPIPE_LANE_ALIGN_OFF_MASK                 (0x1 << HPIPE_LANE_ALIGN_OFF_OFFSET)
+
+#define HPIPE_MISC_REG                            0x13C
+#define HPIPE_MISC_CLK100M_125M_OFFSET            4
+#define HPIPE_MISC_CLK100M_125M_MASK              (0x1 << HPIPE_MISC_CLK100M_125M_OFFSET)
+#define HPIPE_MISC_ICP_FORCE_OFFSET               5
+#define HPIPE_MISC_ICP_FORCE_MASK                 (0x1 << HPIPE_MISC_ICP_FORCE_OFFSET)
+#define HPIPE_MISC_TXDCLK_2X_OFFSET               6
+#define HPIPE_MISC_TXDCLK_2X_MASK                 (0x1 << HPIPE_MISC_TXDCLK_2X_OFFSET)
+#define HPIPE_MISC_CLK500_EN_OFFSET               7
+#define HPIPE_MISC_CLK500_EN_MASK                 (0x1 << HPIPE_MISC_CLK500_EN_OFFSET)
+#define HPIPE_MISC_REFCLK_SEL_OFFSET              10
+#define HPIPE_MISC_REFCLK_SEL_MASK                (0x1 << HPIPE_MISC_REFCLK_SEL_OFFSET)
+
+#define HPIPE_RX_CONTROL_1_REG                    0x140
+#define HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET     11
+#define HPIPE_RX_CONTROL_1_RXCLK2X_SEL_MASK       (0x1 << HPIPE_RX_CONTROL_1_RXCLK2X_SEL_OFFSET)
+#define HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET        12
+#define HPIPE_RX_CONTROL_1_CLK8T_EN_MASK          (0x1 << HPIPE_RX_CONTROL_1_CLK8T_EN_OFFSET)
+
+#define HPIPE_PWR_CTR_REG                         0x148
+#define HPIPE_PWR_CTR_RST_DFE_OFFSET              0
+#define HPIPE_PWR_CTR_RST_DFE_MASK                (0x1 << HPIPE_PWR_CTR_RST_DFE_OFFSET)
+#define HPIPE_PWR_CTR_SFT_RST_OFFSET              10
+#define HPIPE_PWR_CTR_SFT_RST_MASK                (0x1 << HPIPE_PWR_CTR_SFT_RST_OFFSET)
+
+#define HPIPE_PLLINTP_REG1                        0x150
+
+#define HPIPE_SPD_DIV_FORCE_REG                       0x154
+#define HPIPE_TXDIGCK_DIV_FORCE_OFFSET                7
+#define HPIPE_TXDIGCK_DIV_FORCE_MASK                  (0x1 << HPIPE_TXDIGCK_DIV_FORCE_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET         8
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_MASK           (0x3 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET   10
+#define HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_MASK     (0x1 << HPIPE_SPD_DIV_FORCE_RX_SPD_DIV_FORCE_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET         13
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_MASK           (0x3 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_OFFSET)
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET   15
+#define HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_MASK     (0x1 << HPIPE_SPD_DIV_FORCE_TX_SPD_DIV_FORCE_OFFSET)
+
+#define HPIPE_SAMPLER_N_PROC_CALIB_CTRL_REG       0x16C
+#define HPIPE_RX_SAMPLER_OS_GAIN_OFFSET           6
+#define HPIPE_RX_SAMPLER_OS_GAIN_MASK             (0x3 << HPIPE_RX_SAMPLER_OS_GAIN_OFFSET)
+#define HPIPE_SAMPLER_OFFSET                      12
+#define HPIPE_SAMPLER_MASK                        (0x1 << HPIPE_SAMPLER_OFFSET)
+
+#define HPIPE_TX_REG1_REG                         0x174
+#define HPIPE_TX_REG1_TX_EMPH_RES_OFFSET          5
+#define HPIPE_TX_REG1_TX_EMPH_RES_MASK            (0x3 << HPIPE_TX_REG1_TX_EMPH_RES_OFFSET)
+#define HPIPE_TX_REG1_SLC_EN_OFFSET               10
+#define HPIPE_TX_REG1_SLC_EN_MASK                 (0x3f << HPIPE_TX_REG1_SLC_EN_OFFSET)
+
+#define HPIPE_TX_REG1_REG                         0x174
+#define HPIPE_TX_REG1_TX_EMPH_RES_OFFSET          5
+#define HPIPE_TX_REG1_TX_EMPH_RES_MASK            (0x3 << HPIPE_TX_REG1_TX_EMPH_RES_OFFSET)
+#define HPIPE_TX_REG1_SLC_EN_OFFSET               10
+#define HPIPE_TX_REG1_SLC_EN_MASK                 (0x3f << HPIPE_TX_REG1_SLC_EN_OFFSET)
+
+#define HPIPE_PWR_CTR_DTL_REG                         0x184
+#define HPIPE_PWR_CTR_DTL_SQ_DET_EN_OFFSET            0
+#define HPIPE_PWR_CTR_DTL_SQ_DET_EN_MASK              (0x1 << HPIPE_PWR_CTR_DTL_SQ_DET_EN_OFFSET)
+#define HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_OFFSET          1
+#define HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_MASK            (0x1 << HPIPE_PWR_CTR_DTL_SQ_PLOOP_EN_OFFSET)
+#define HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET             2
+#define HPIPE_PWR_CTR_DTL_FLOOP_EN_MASK               (0x1 << HPIPE_PWR_CTR_DTL_FLOOP_EN_OFFSET)
+#define HPIPE_PWR_CTR_DTL_CLAMPING_SEL_OFFSET         4
+#define HPIPE_PWR_CTR_DTL_CLAMPING_SEL_MASK           (0x7 << HPIPE_PWR_CTR_DTL_CLAMPING_SEL_OFFSET)
+#define HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_OFFSET    10
+#define HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_MASK      (0x1 << HPIPE_PWR_CTR_DTL_INTPCLK_DIV_FORCE_OFFSET)
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_OFFSET             12
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_MASK               (0x3 << HPIPE_PWR_CTR_DTL_CLK_MODE_OFFSET)
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_OFFSET       14
+#define HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_MASK         (1 << HPIPE_PWR_CTR_DTL_CLK_MODE_FORCE_OFFSET)
+
+#define HPIPE_PHASE_CONTROL_REG                   0x188
+#define HPIPE_OS_PH_OFFSET_OFFSET                 0
+#define HPIPE_OS_PH_OFFSET_MASK                   (0x7f << HPIPE_OS_PH_OFFSET_OFFSET)
+#define HPIPE_OS_PH_OFFSET_FORCE_OFFSET           7
+#define HPIPE_OS_PH_OFFSET_FORCE_MASK             (0x1 << HPIPE_OS_PH_OFFSET_FORCE_OFFSET)
+#define HPIPE_OS_PH_VALID_OFFSET                  8
+#define HPIPE_OS_PH_VALID_MASK                    (0x1 << HPIPE_OS_PH_VALID_OFFSET)
+
+#define HPIPE_FRAME_DETECT_CTRL_0_REG             0x214
+#define HPIPE_TRAIN_PAT_NUM_OFFSET                0x7
+#define HPIPE_TRAIN_PAT_NUM_MASK                  (0x1FF << HPIPE_TRAIN_PAT_NUM_OFFSET)
+
+#define HPIPE_FRAME_DETECT_CTRL_3_REG             0x220
+#define HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_OFFSET 12
+#define HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_MASK   (0x1 << HPIPE_PATTERN_LOCK_LOST_TIMEOUT_EN_OFFSET)
+
+#define HPIPE_DME_REG                             0x228
+#define HPIPE_DME_ETHERNET_MODE_OFFSET            7
+#define HPIPE_DME_ETHERNET_MODE_MASK              (0x1 << HPIPE_DME_ETHERNET_MODE_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_0_REG                 0x268
+#define HPIPE_TX_TRAIN_P2P_HOLD_OFFSET            15
+#define HPIPE_TX_TRAIN_P2P_HOLD_MASK              (0x1 << HPIPE_TX_TRAIN_P2P_HOLD_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_REG                   0x26C
+#define HPIPE_TX_TRAIN_CTRL_G1_OFFSET             0
+#define HPIPE_TX_TRAIN_CTRL_G1_MASK               (0x1 << HPIPE_TX_TRAIN_CTRL_G1_OFFSET)
+#define HPIPE_TX_TRAIN_CTRL_GN1_OFFSET            1
+#define HPIPE_TX_TRAIN_CTRL_GN1_MASK              (0x1 << HPIPE_TX_TRAIN_CTRL_GN1_OFFSET)
+#define HPIPE_TX_TRAIN_CTRL_G0_OFFSET             2
+#define HPIPE_TX_TRAIN_CTRL_G0_MASK               (0x1 << HPIPE_TX_TRAIN_CTRL_G0_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_4_REG                 0x278
+#define HPIPE_TRX_TRAIN_TIMER_OFFSET              0
+#define HPIPE_TRX_TRAIN_TIMER_MASK                (0x3FF << HPIPE_TRX_TRAIN_TIMER_OFFSET)
+
+#define HPIPE_PCIE_REG1                           0x288
+#define HPIPE_PCIE_REG3                           0x290
+
+#define HPIPE_TX_TRAIN_CTRL_5_REG                 0x2A4
+#define HPIPE_RX_TRAIN_TIMER_OFFSET               0
+#define HPIPE_RX_TRAIN_TIMER_MASK                 (0x3ff << HPIPE_RX_TRAIN_TIMER_OFFSET)
+#define HPIPE_TX_TRAIN_START_SQ_EN_OFFSET         11
+#define HPIPE_TX_TRAIN_START_SQ_EN_MASK           (0x1 << HPIPE_TX_TRAIN_START_SQ_EN_OFFSET)
+#define HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET    12
+#define HPIPE_TX_TRAIN_START_FRM_DET_EN_MASK      (0x1 << HPIPE_TX_TRAIN_START_FRM_DET_EN_OFFSET)
+#define HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET   13
+#define HPIPE_TX_TRAIN_START_FRM_LOCK_EN_MASK     (0x1 << HPIPE_TX_TRAIN_START_FRM_LOCK_EN_OFFSET)
+#define HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET        14
+#define HPIPE_TX_TRAIN_WAIT_TIME_EN_MASK          (0x1 << HPIPE_TX_TRAIN_WAIT_TIME_EN_OFFSET)
+
+#define HPIPE_TX_TRAIN_REG                        0x31C
+#define HPIPE_TX_TRAIN_CHK_INIT_OFFSET            4
+#define HPIPE_TX_TRAIN_CHK_INIT_MASK              (0x1 << HPIPE_TX_TRAIN_CHK_INIT_OFFSET)
+#define HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET    7
+#define HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_MASK      (0x1 << HPIPE_TX_TRAIN_COE_FM_PIN_PCIE3_OFFSET)
+#define HPIPE_TX_TRAIN_16BIT_AUTO_EN_OFFSET       8
+#define HPIPE_TX_TRAIN_16BIT_AUTO_EN_MASK         (0x1 << HPIPE_TX_TRAIN_16BIT_AUTO_EN_OFFSET)
+#define HPIPE_TX_TRAIN_PAT_SEL_OFFSET             9
+#define HPIPE_TX_TRAIN_PAT_SEL_MASK               (0x1 << HPIPE_TX_TRAIN_PAT_SEL_OFFSET)
+
+#define HPIPE_CDR_CONTROL_REG                     0x418
+#define HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET          6
+#define HPIPE_CDR_MAX_DFE_ADAPT_1_MASK            (0x7 << HPIPE_CDR_MAX_DFE_ADAPT_1_OFFSET)
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET          9
+#define HPIPE_CDR_MAX_DFE_ADAPT_0_MASK            (0x7 << HPIPE_CDR_MAX_DFE_ADAPT_0_OFFSET)
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET       12
+#define HPIPE_CDR_RX_MAX_DFE_ADAPT_1_MASK         (0x3 << HPIPE_CDR_RX_MAX_DFE_ADAPT_1_OFFSET)
+
+#define HPIPE_TX_TRAIN_CTRL_11_REG                0x438
+#define HPIPE_TX_STATUS_CHECK_MODE_OFFSET         6
+#define HPIPE_TX_TX_STATUS_CHECK_MODE_MASK        (0x1 << HPIPE_TX_STATUS_CHECK_MODE_OFFSET)
+#define HPIPE_TX_NUM_OF_PRESET_OFFSET             10
+#define HPIPE_TX_NUM_OF_PRESET_MASK               (0x7 << HPIPE_TX_NUM_OF_PRESET_OFFSET)
+#define HPIPE_TX_SWEEP_PRESET_EN_OFFSET           15
+#define HPIPE_TX_SWEEP_PRESET_EN_MASK             (0x1 << HPIPE_TX_SWEEP_PRESET_EN_OFFSET)
+
+#define HPIPE_G1_SET3_REG                         0x440
+#define HPIPE_G2_SET3_REG                         0x448
+#define HPIPE_G3_SET3_REG                         0x450
+#define HPIPE_GX_SET3_FFE_CAP_SEL_OFFSET          0
+#define HPIPE_GX_SET3_FFE_CAP_SEL_MASK            (0xf << HPIPE_GX_SET3_FFE_CAP_SEL_OFFSET)
+#define HPIPE_GX_SET3_FFE_RES_SEL_OFFSET          4
+#define HPIPE_GX_SET3_FFE_RES_SEL_MASK            (0x7 << HPIPE_GX_SET3_FFE_RES_SEL_OFFSET)
+#define HPIPE_GX_SET3_FFE_SETTING_FORCE_OFFSET    7
+#define HPIPE_GX_SET3_FFE_SETTING_FORCE_MASK      (0x1 << HPIPE_GX_SET3_FFE_SETTING_FORCE_OFFSET)
+#define HPIPE_GX_SET3_FBCK_SEL_OFFSET             9
+#define HPIPE_GX_SET3_FBCK_SEL_MASK               (0x1 << HPIPE_GX_SET3_FBCK_SEL_OFFSET)
+#define HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_OFFSET    12
+#define HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_MASK      (0x3 << HPIPE_GX_SET3_FFE_DEG_RES_LEVEL_OFFSET)
+#define HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_OFFSET   14
+#define HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_MASK     (0x3 << HPIPE_GX_SET3_FFE_LOAD_RES_LEVEL_OFFSET)
+
+#define HPIPE_G1_SET4_REG                         0x444
+#define HPIPE_G2_SET4_REG                         0x44C
+#define HPIPE_G3_SET4_REG                         0x454
+#define HPIPE_GX_SET4_DFE_RES_OFFSET              8
+#define HPIPE_GX_SET4_DFE_RES_MASK                (0x3 << HPIPE_GX_SET4_DFE_RES_OFFSET)
+
+#define HPIPE_TX_PRESET_INDEX_REG                 0x468
+#define HPIPE_TX_PRESET_INDEX_OFFSET              0
+#define HPIPE_TX_PRESET_INDEX_MASK                (0xf << HPIPE_TX_PRESET_INDEX_OFFSET)
+
+#define HPIPE_DFE_CONTROL_REG                     0x470
+#define HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET         14
+#define HPIPE_DFE_TX_MAX_DFE_ADAPT_MASK           (0x3 << HPIPE_DFE_TX_MAX_DFE_ADAPT_OFFSET)
+
+#define HPIPE_DFE_CTRL_28_REG                     0x49C
+#define HPIPE_DFE_CTRL_28_PIPE4_OFFSET            7
+#define HPIPE_DFE_CTRL_28_PIPE4_MASK              (0x1 << HPIPE_DFE_CTRL_28_PIPE4_OFFSET)
+
+#define HPIPE_G1_SET5_REG                         0x538
+#define HPIPE_G3_SET5_REG                         0x548
+#define HPIPE_GX_SET5_ICP_OFFSET                  0
+#define HPIPE_GX_SET5_ICP_MASK                    (0xf << HPIPE_GX_SET5_ICP_OFFSET)
+
+#define HPIPE_LANE_CONFIG0_REG                    0x604
+#define HPIPE_LANE_CONFIG0_MAX_PLL_OFFSET         9
+#define HPIPE_LANE_CONFIG0_MAX_PLL_MASK           (0x1 << HPIPE_LANE_CONFIG0_MAX_PLL_OFFSET)
+#define HPIPE_LANE_CONFIG0_GEN2_PLL_OFFSET        10
+#define HPIPE_LANE_CONFIG0_GEN2_PLL_MASK          (0x1 << HPIPE_LANE_CONFIG0_GEN2_PLL_OFFSET)
+
+#define HPIPE_LANE_STATUS0_REG                    0x60C
+#define HPIPE_LANE_STATUS0_PCLK_EN_OFFSET         0
+#define HPIPE_LANE_STATUS0_PCLK_EN_MASK           (0x1 << HPIPE_LANE_STATUS0_PCLK_EN_OFFSET)
+
+#define HPIPE_LANE_CFG4_REG                       0x620
+#define HPIPE_LANE_CFG4_DFE_CTRL_OFFSET           0
+#define HPIPE_LANE_CFG4_DFE_CTRL_MASK             (0x7 << HPIPE_LANE_CFG4_DFE_CTRL_OFFSET)
+#define HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET         3
+#define HPIPE_LANE_CFG4_DFE_EN_SEL_MASK           (0x1 << HPIPE_LANE_CFG4_DFE_EN_SEL_OFFSET)
+#define HPIPE_LANE_CFG4_DFE_OVER_OFFSET           6
+#define HPIPE_LANE_CFG4_DFE_OVER_MASK             (0x1 << HPIPE_LANE_CFG4_DFE_OVER_OFFSET)
+#define HPIPE_LANE_CFG4_SSC_CTRL_OFFSET           7
+#define HPIPE_LANE_CFG4_SSC_CTRL_MASK             (0x1 << HPIPE_LANE_CFG4_SSC_CTRL_OFFSET)
+
+#define HPIPE_LANE_EQU_CONFIG_0_REG               0x69C
+#define HPIPE_CFG_PHY_RC_EP_OFFSET                12
+#define HPIPE_CFG_PHY_RC_EP_MASK                  (0x1 << HPIPE_CFG_PHY_RC_EP_OFFSET)
+
+#define HPIPE_LANE_EQ_CFG1_REG                    0x6a0
+#define HPIPE_CFG_UPDATE_POLARITY_OFFSET          12
+#define HPIPE_CFG_UPDATE_POLARITY_MASK            (0x1 << HPIPE_CFG_UPDATE_POLARITY_OFFSET)
+
+#define HPIPE_LANE_EQ_REMOTE_SETTING_REG          0x6f8
+#define HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET   0
+#define HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_MASK     (0x1 << HPIPE_LANE_CFG_FOM_DIRN_OVERRIDE_OFFSET)
+#define HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET      1
+#define HPIPE_LANE_CFG_FOM_ONLY_MODE_MASK         (0x1 << HPIPE_LANE_CFG_FOM_ONLY_MODE_OFFFSET)
+#define HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET   2
+#define HPIPE_LANE_CFG_FOM_PRESET_VECTOR_MASK     (0xf << HPIPE_LANE_CFG_FOM_PRESET_VECTOR_OFFSET)
+
+#define HPIPE_RST_CLK_CTRL_REG                    0x704
+#define HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET        0
+#define HPIPE_RST_CLK_CTRL_PIPE_RST_MASK          (0x1 << HPIPE_RST_CLK_CTRL_PIPE_RST_OFFSET)
+#define HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET      2
+#define HPIPE_RST_CLK_CTRL_FIXED_PCLK_MASK        (0x1 << HPIPE_RST_CLK_CTRL_FIXED_PCLK_OFFSET)
+#define HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET      3
+#define HPIPE_RST_CLK_CTRL_PIPE_WIDTH_MASK        (0x1 << HPIPE_RST_CLK_CTRL_PIPE_WIDTH_OFFSET)
+#define HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET   9
+#define HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_MASK     (0x1 << HPIPE_RST_CLK_CTRL_CORE_FREQ_SEL_OFFSET)
+
+#define HPIPE_CLK_SRC_LO_REG                      0x70c
+#define HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET        5
+#define HPIPE_CLK_SRC_LO_PLL_RDY_DL_MASK          (0x7 << HPIPE_CLK_SRC_LO_PLL_RDY_DL_OFFSET)
+
+#define HPIPE_CLK_SRC_HI_REG                      0x710
+#define HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET         0
+#define HPIPE_CLK_SRC_HI_LANE_STRT_MASK           (0x1 << HPIPE_CLK_SRC_HI_LANE_STRT_OFFSET)
+#define HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET        1
+#define HPIPE_CLK_SRC_HI_LANE_BREAK_MASK          (0x1 << HPIPE_CLK_SRC_HI_LANE_BREAK_OFFSET)
+#define HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET       2
+#define HPIPE_CLK_SRC_HI_LANE_MASTER_MASK         (0x1 << HPIPE_CLK_SRC_HI_LANE_MASTER_OFFSET)
+#define HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET         7
+#define HPIPE_CLK_SRC_HI_MODE_PIPE_MASK           (0x1 << HPIPE_CLK_SRC_HI_MODE_PIPE_OFFSET)
+
+#define HPIPE_GLOBAL_MISC_CTRL                    0x718
+#define HPIPE_GLOBAL_PM_CTRL                      0x740
+#define HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET        0
+#define HPIPE_GLOBAL_PM_RXDLOZ_WAIT_MASK          (0xFF << HPIPE_GLOBAL_PM_RXDLOZ_WAIT_OFFSET)
+
+/***** COMPHY registers *****/
+#define COMMON_PHY_CFG1_REG                       0x0
+#define COMMON_PHY_CFG1_PWR_UP_OFFSET             1
+#define COMMON_PHY_CFG1_PWR_UP_MASK               (0x1 << COMMON_PHY_CFG1_PWR_UP_OFFSET)
+#define COMMON_PHY_CFG1_PIPE_SELECT_OFFSET        2
+#define COMMON_PHY_CFG1_PIPE_SELECT_MASK          (0x1 << COMMON_PHY_CFG1_PIPE_SELECT_OFFSET)
+#define COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET       13
+#define COMMON_PHY_CFG1_PWR_ON_RESET_MASK         (0x1 << COMMON_PHY_CFG1_PWR_ON_RESET_OFFSET)
+#define COMMON_PHY_CFG1_CORE_RSTN_OFFSET          14
+#define COMMON_PHY_CFG1_CORE_RSTN_MASK            (0x1 << COMMON_PHY_CFG1_CORE_RSTN_OFFSET)
+#define COMMON_PHY_PHY_MODE_OFFSET                15
+#define COMMON_PHY_PHY_MODE_MASK                  (0x1 << COMMON_PHY_PHY_MODE_OFFSET)
+
+#define COMMON_PHY_CFG6_REG                       0x14
+#define COMMON_PHY_CFG6_IF_40_SEL_OFFSET          18
+#define COMMON_PHY_CFG6_IF_40_SEL_MASK            (0x1 << COMMON_PHY_CFG6_IF_40_SEL_OFFSET)
+
+#define COMMON_SELECTOR_PHY_OFFSET                0x140
+#define COMMON_SELECTOR_PIPE_OFFSET               0x144
+
+#define COMMON_PHY_SD_CTRL1                       0x148
+#define COMMON_PHY_SD_CTRL1_RXAUI1_OFFSET         26
+#define COMMON_PHY_SD_CTRL1_RXAUI1_MASK           (0x1 << COMMON_PHY_SD_CTRL1_RXAUI1_OFFSET)
+#define COMMON_PHY_SD_CTRL1_RXAUI0_OFFSET         27
+#define COMMON_PHY_SD_CTRL1_RXAUI0_MASK           (0x1 << COMMON_PHY_SD_CTRL1_RXAUI0_OFFSET)
+
+/***** SATA registers *****/
+#define SATA3_VENDOR_ADDRESS                      0xA0
+#define SATA3_VENDOR_ADDR_OFSSET                  0
+#define SATA3_VENDOR_ADDR_MASK                    (0xFFFFFFFF << SATA3_VENDOR_ADDR_OFSSET)
+#define SATA3_VENDOR_DATA                         0xA4
+
+#define SATA_CONTROL_REG                          0x0
+#define SATA3_CTRL_SATA0_PD_OFFSET                6
+#define SATA3_CTRL_SATA0_PD_MASK                  (1 << SATA3_CTRL_SATA0_PD_OFFSET)
+#define SATA3_CTRL_SATA1_PD_OFFSET                14
+#define SATA3_CTRL_SATA1_PD_MASK                  (1 << SATA3_CTRL_SATA1_PD_OFFSET)
+#define SATA3_CTRL_SATA1_ENABLE_OFFSET            22
+#define SATA3_CTRL_SATA1_ENABLE_MASK              (1 << SATA3_CTRL_SATA1_ENABLE_OFFSET)
+#define SATA3_CTRL_SATA_SSU_OFFSET                23
+#define SATA3_CTRL_SATA_SSU_MASK                  (1 << SATA3_CTRL_SATA_SSU_OFFSET)
+
+#define SATA_MBUS_SIZE_SELECT_REG                 0x4
+#define SATA_MBUS_REGRET_EN_OFFSET                7
+#define SATA_MBUS_REGRET_EN_MASK                  (0x1 << SATA_MBUS_REGRET_EN_OFFSET)
+
+/***************************/
+
+typedef struct _CHIP_COMPHY_CONFIG CHIP_COMPHY_CONFIG;
+
+typedef struct {
+  UINT32 Type;
+  UINT32 MuxValue;
+} COMPHY_MUX_OPTIONS;
+
+typedef struct {
+  UINT32 MaxLaneValues;
+  COMPHY_MUX_OPTIONS MuxValues[MAX_LANE_OPTIONS];
+} COMPHY_MUX_DATA;
+
+typedef struct {
+  UINT32 Type;
+  UINT32 Speed;
+  UINT32 Invert;
+} COMPHY_MAP;
+
+typedef struct {
+  CHAR16 *TypeStr[MAX_LANE_OPTIONS];
+  UINTN SpeedValue[MAX_LANE_OPTIONS];
+  UINTN InvFlag[MAX_LANE_OPTIONS];
+} PCD_LANE_MAP;
+
+typedef
+VOID
+(*COMPHY_CHIP_INIT) (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg
+  );
+
+struct _CHIP_COMPHY_CONFIG {
+  MV_COMPHY_CHIP_TYPE ChipType;
+  COMPHY_MAP MapData[MAX_LANE_OPTIONS];
+  COMPHY_MUX_DATA *MuxData;
+  EFI_PHYSICAL_ADDRESS ComPhyBaseAddr;
+  EFI_PHYSICAL_ADDRESS Hpipe3BaseAddr;
+  COMPHY_CHIP_INIT Init;
+  UINT32 LanesCount;
+  UINT32 MuxBitCount;
+};
+
+VOID
+ComPhyMuxInit (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg,
+  IN COMPHY_MAP *ComPhyMapData,
+  IN EFI_PHYSICAL_ADDRESS SelectorBase
+  );
+
+VOID
+ComPhyCp110Init (
+  IN CHIP_COMPHY_CONFIG * First
+  );
+
+VOID
+RegSet (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Data,
+  IN UINT32 Mask
+  );
+
+VOID
+RegSetSilent (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Data,
+  IN UINT32 Mask
+  );
+
+VOID
+RegSet16 (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT16 Data,
+  IN UINT16 Mask
+  );
+
+VOID
+RegSetSilent16(
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT16 Data,
+  IN UINT16 Mask
+  );
+#endif // __COMPHY_H__
diff --git a/Platform/Marvell/Library/ComPhyLib/ComPhyLib.inf b/Platform/Marvell/Library/ComPhyLib/ComPhyLib.inf
new file mode 100644
index 0000000000..e0f4634dca
--- /dev/null
+++ b/Platform/Marvell/Library/ComPhyLib/ComPhyLib.inf
@@ -0,0 +1,84 @@
+# Copyright (C) 2016 Marvell International Ltd.
+#
+# Marvell BSD License Option
+#
+# If you received this File from Marvell, you may opt to use, redistribute and/or
+# modify this File under the following licensing terms.
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright
+#   notice, this list of conditions and the following disclaimer in the
+#   documentation and/or other materials provided with the distribution.
+#
+# * Neither the name of Marvell nor the names of its contributors may be
+#   used to endorse or promote products derived from this software without
+#   specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = MarvellComPhyLib
+  FILE_GUID                      = 3314541a-9647-4a37-b8c6-24e000900e4e
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ComPhyLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  Platform/Marvell/Marvell.dec
+
+[LibraryClasses]
+  ArmLib
+  DebugLib
+  MemoryAllocationLib
+  PcdLib
+  IoLib
+  ParsePcdLib
+
+[Sources.common]
+  ComPhyLib.c
+  ComPhyCp110.c
+  ComPhyMux.c
+
+[FixedPcd]
+  gMarvellTokenSpaceGuid.PcdComPhyDevices
+
+  #Chip0
+  gMarvellTokenSpaceGuid.PcdChip0ComPhyTypes
+  gMarvellTokenSpaceGuid.PcdChip0ComPhySpeeds
+  gMarvellTokenSpaceGuid.PcdChip0ComPhyInvFlags
+
+  #Chip1
+  gMarvellTokenSpaceGuid.PcdChip1ComPhyTypes
+  gMarvellTokenSpaceGuid.PcdChip1ComPhySpeeds
+  gMarvellTokenSpaceGuid.PcdChip1ComPhyInvFlags
+
+  #Chip2
+  gMarvellTokenSpaceGuid.PcdChip2ComPhyTypes
+  gMarvellTokenSpaceGuid.PcdChip2ComPhySpeeds
+  gMarvellTokenSpaceGuid.PcdChip2ComPhyInvFlags
+
+  #Chip3
+  gMarvellTokenSpaceGuid.PcdChip3ComPhyTypes
+  gMarvellTokenSpaceGuid.PcdChip3ComPhySpeeds
+  gMarvellTokenSpaceGuid.PcdChip3ComPhyInvFlags
+
+  gMarvellTokenSpaceGuid.PcdPciEAhci
diff --git a/Platform/Marvell/Library/ComPhyLib/ComPhyMux.c b/Platform/Marvell/Library/ComPhyLib/ComPhyMux.c
new file mode 100644
index 0000000000..6589fec9f0
--- /dev/null
+++ b/Platform/Marvell/Library/ComPhyLib/ComPhyMux.c
@@ -0,0 +1,132 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#include "ComPhyLib.h"
+
+STATIC
+VOID
+ComPhyMuxCheckConfig (
+  IN COMPHY_MUX_DATA *MuxData,
+  IN COMPHY_MAP *ComPhyMapData,
+  IN UINTN ComPhyMaxLanes
+  )
+{
+  COMPHY_MUX_OPTIONS *PtrMuxOpt;
+  UINTN Lane, Opt, Valid;
+
+  for (Lane = 0; Lane < ComPhyMaxLanes; Lane++, ComPhyMapData++, MuxData++) {
+    PtrMuxOpt = MuxData->MuxValues;
+    for (Opt = 0, Valid = 0; Opt < MuxData->MaxLaneValues; Opt++, PtrMuxOpt++) {
+      if (PtrMuxOpt->Type == ComPhyMapData->Type) {
+        Valid = 1;
+        break;
+      }
+    }
+    if (Valid == 0) {
+      DEBUG((DEBUG_INFO, "Lane number %d, had invalid Type %d\n", Lane,
+        ComPhyMapData->Type));
+      DEBUG((DEBUG_INFO, "Set Lane %d as Type %d\n", Lane,
+        COMPHY_TYPE_UNCONNECTED));
+      ComPhyMapData->Type = COMPHY_TYPE_UNCONNECTED;
+    } else {
+      DEBUG((DEBUG_INFO, "Lane number %d, has Type %d\n", Lane,
+        ComPhyMapData->Type));
+    }
+  }
+}
+
+STATIC
+UINT32
+ComPhyMuxGetMuxValue (
+  IN COMPHY_MUX_DATA *MuxData,
+  IN UINT32 Type,
+  IN UINTN Lane
+  )
+{
+  COMPHY_MUX_OPTIONS *PtrMuxOpt;
+  UINTN Opt;
+  UINT32 Value = 0;
+
+  PtrMuxOpt = MuxData->MuxValues;
+  for (Opt = 0 ; Opt < MuxData->MaxLaneValues; Opt++, PtrMuxOpt++)
+    if (PtrMuxOpt->Type == Type) {
+      Value = PtrMuxOpt->MuxValue;
+      break;
+    }
+
+  return Value;
+}
+
+STATIC
+VOID
+ComPhyMuxRegWrite (
+  IN COMPHY_MUX_DATA *MuxData,
+  IN COMPHY_MAP *ComPhyMapData,
+  IN UINTN ComPhyMaxLanes,
+  IN EFI_PHYSICAL_ADDRESS SelectorBase,
+  IN UINT32 BitCount
+  )
+{
+  UINT32 Lane, Value, Offset, Mask;
+
+  for (Lane = 0; Lane < ComPhyMaxLanes; Lane++, ComPhyMapData++, MuxData++) {
+    Offset = Lane * BitCount;
+    Mask = (((1 << BitCount) - 1) << Offset);
+    Value = (ComPhyMuxGetMuxValue (MuxData, ComPhyMapData->Type, Lane) <<
+      Offset);
+    RegSet (SelectorBase, Value, Mask);
+  }
+}
+
+VOID
+ComPhyMuxInit (
+  IN CHIP_COMPHY_CONFIG *PtrChipCfg,
+  IN COMPHY_MAP *ComPhyMapData,
+  IN EFI_PHYSICAL_ADDRESS SelectorBase
+  )
+{
+  COMPHY_MUX_DATA *MuxData;
+  UINT32 MuxBitCount;
+  UINT32 ComPhyMaxLanes;
+
+  ComPhyMaxLanes = PtrChipCfg->LanesCount;
+  MuxData = PtrChipCfg->MuxData;
+  MuxBitCount = PtrChipCfg->MuxBitCount;
+
+  /* Check if the configuration is valid */
+  ComPhyMuxCheckConfig (MuxData, ComPhyMapData, ComPhyMaxLanes);
+  /* Init COMPHY selectors */
+  ComPhyMuxRegWrite (MuxData, ComPhyMapData, ComPhyMaxLanes, SelectorBase,
+    MuxBitCount);
+}
diff --git a/Platform/Marvell/Library/MppLib/MppLib.c b/Platform/Marvell/Library/MppLib/MppLib.c
new file mode 100644
index 0000000000..c09acf95fe
--- /dev/null
+++ b/Platform/Marvell/Library/MppLib/MppLib.c
@@ -0,0 +1,216 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#include <Library/ArmLib.h>
+#include <Library/ArmPlatformLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/IoLib.h>
+
+#define MPP_PIN_VAL(pin,func)    (((func) & 0xf) << ((pin) * 4))
+#define MPP_MAX_REGS             8
+#define MPP_PINS_PER_REG         8
+#define PCD_PINS_PER_GROUP       10
+
+#define SD_MMC_PHY_AP_MPP_OFFSET   0x100
+#define SD_MMC_PHY_CP0_MPP_OFFSET  0x424
+#define MPP_ON_SDPHY_ENABLE        (1 << 0)
+
+#define MAX_CHIPS                4
+
+#define GET_PCD_PTR(id,num)      PcdGetPtr(PcdChip##id##MppSel##num)
+#define GET_PIN_COUNT(id)        PcdGet32(PcdChip##id##MppPinCount)
+#define GET_BASE(id)             PcdGet64(PcdChip##id##MppBaseAddress)
+#define GET_REV_FLAG(id)         PcdGetBool(PcdChip##id##MppReverseFlag)
+
+/* We get chip number */
+#define GetMppPcd(id) {                 \
+  PinCount[id] = GET_PIN_COUNT(id);     \
+  MppRegPcd[id][7] = GET_PCD_PTR(id,7); \
+  MppRegPcd[id][6] = GET_PCD_PTR(id,6); \
+  MppRegPcd[id][5] = GET_PCD_PTR(id,5); \
+  MppRegPcd[id][4] = GET_PCD_PTR(id,4); \
+  MppRegPcd[id][3] = GET_PCD_PTR(id,3); \
+  MppRegPcd[id][2] = GET_PCD_PTR(id,2); \
+  MppRegPcd[id][1] = GET_PCD_PTR(id,1); \
+  MppRegPcd[id][0] = GET_PCD_PTR(id,0); \
+  BaseAddr[id] = GET_BASE(id);          \
+  ReverseFlag[id] = GET_REV_FLAG(id);   \
+}
+
+STATIC
+VOID
+SetRegisterValue (
+  UINT8 RegCount,
+  UINT8 **MppRegPcd,
+  UINTN BaseAddr,
+  BOOLEAN ReverseFlag
+  )
+{
+  UINT32 i, j, CtrlVal;
+  INTN Sign;
+
+  Sign = ReverseFlag ? -1 : 1;
+
+  for (i = 0; i < RegCount; i++) {
+    CtrlVal = 0;
+    for (j = 0; j < MPP_PINS_PER_REG; j++) {
+      CtrlVal |= MPP_PIN_VAL(7 * (UINTN) ReverseFlag + j * Sign,
+        MppRegPcd[i][7 * (UINTN) ReverseFlag + j * Sign]);
+    }
+    MmioWrite32 (BaseAddr + 4 * i * Sign, CtrlVal);
+  }
+}
+
+STATIC
+/* Transform PCD MPP group format into hardware register format */
+UINT8
+PcdToMppRegs (
+  UINTN PinCount,
+  UINT8 **MppRegPcd
+  )
+{
+  UINT8 MppRegPcdTmp[MPP_MAX_REGS][MPP_PINS_PER_REG];
+  UINT8 PcdGroupCount, MppRegCount;
+  UINTN i, j, k, l;
+
+  if (PinCount == 0) {
+    return 0;
+  }
+
+  PcdGroupCount = PinCount / PCD_PINS_PER_GROUP;
+  if ((PinCount % PCD_PINS_PER_GROUP) != 0) {
+    PcdGroupCount += 1;
+  }
+
+  MppRegCount = PinCount / MPP_PINS_PER_REG;
+  if ((PinCount % MPP_PINS_PER_REG) != 0) {
+    MppRegCount += 1;
+  }
+
+  /* Fill temporary table with data from PCD groups in HW format */
+  for (i = 0; i < PcdGroupCount; i++) {
+    for (j = 0; j < PCD_PINS_PER_GROUP; j++) {
+      k = (PCD_PINS_PER_GROUP * i + j) / MPP_PINS_PER_REG;
+      l = (PCD_PINS_PER_GROUP * i + j) % MPP_PINS_PER_REG;
+      MppRegPcdTmp[k][l] = MppRegPcd[i][j];
+    }
+  }
+
+  /* Update input table */
+  for (i = 0; i < MppRegCount; i++) {
+    for (j = 0; j < MPP_PINS_PER_REG; j++) {
+      MppRegPcd[i][j] = MppRegPcdTmp[i][j];
+    }
+  }
+
+  return MppRegCount;
+}
+
+STATIC
+VOID
+SetSdMmcPhyMpp (
+  UINTN  BaseAddr,
+  UINT32 Index
+  )
+{
+  UINTN  Size, Offset;
+  UINT8 *Ptr;
+  UINT32 Reg;
+
+  Size = PcdGetSize(PcdPciESdhci);
+  Ptr = (UINT8 *) PcdGetPtr(PcdPciESdhci);
+
+  if (Ptr == NULL || Index >= Size) {
+    return;
+  }
+
+  /* Check if SDHCI controller is enabled on the HW block */
+  if (Ptr[Index] != 1) {
+    return;
+  }
+
+  /* Choose adequate Offset */
+  switch (Index) {
+  case 0:
+    Offset = SD_MMC_PHY_AP_MPP_OFFSET;
+    break;
+  case 1:
+    Offset = SD_MMC_PHY_CP0_MPP_OFFSET;
+    break;
+  default:
+    return;
+  }
+
+  /*
+   * If there is SDHCI controller on platform, connect SD/MMC PHY to
+   * SD/MMC controller insted of using it as MPP multiplexer
+   */
+  Reg = MmioRead32 (BaseAddr + Offset);
+  Reg &= ~MPP_ON_SDPHY_ENABLE;
+  MmioWrite32 (BaseAddr + Offset, Reg);
+}
+
+EFI_STATUS
+MppInitialize (
+  )
+{
+  UINTN BaseAddr[MAX_CHIPS], PinCount[MAX_CHIPS], RegCount;
+  BOOLEAN ReverseFlag[MAX_CHIPS];
+  UINT8 *MppRegPcd[MAX_CHIPS][MPP_MAX_REGS];
+  UINT32 i, ChipCount;
+
+  ChipCount = PcdGet32 (PcdMppChipCount);
+
+  /* Read all needed PCD for MPP configuration */
+  GetMppPcd(0);
+  GetMppPcd(1);
+  GetMppPcd(2);
+  GetMppPcd(3);
+
+  for (i = 0; i < MAX_CHIPS; i++) {
+    if (i == ChipCount)
+      break;
+    RegCount = PcdToMppRegs (PinCount[i], MppRegPcd[i]);
+    SetRegisterValue (RegCount, MppRegPcd[i], BaseAddr[i], ReverseFlag[i]);
+
+    /*
+     * eMMC PHY IP has its own MPP configuration.
+     */
+    SetSdMmcPhyMpp (BaseAddr[i], i);
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Platform/Marvell/Library/MppLib/MppLib.inf b/Platform/Marvell/Library/MppLib/MppLib.inf
new file mode 100644
index 0000000000..2de9cd0747
--- /dev/null
+++ b/Platform/Marvell/Library/MppLib/MppLib.inf
@@ -0,0 +1,108 @@
+#
+# Marvell BSD License Option
+#
+# If you received this File from Marvell, you may opt to use, redistribute
+# and/or modify this File under the following licensing terms.
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+#
+# * Neither the name of Marvell nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = MarvellMppLib
+  FILE_GUID                      = 3f19b642-4a49-4dfd-8f4a-205dd38432bb
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = MppLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  Platform/Marvell/Marvell.dec
+
+[LibraryClasses]
+  ArmLib
+  DebugLib
+  MemoryAllocationLib
+  PcdLib
+  IoLib
+
+[Sources.common]
+  MppLib.c
+
+[FixedPcd]
+  gMarvellTokenSpaceGuid.PcdMppChipCount
+
+  gMarvellTokenSpaceGuid.PcdChip0MppReverseFlag
+  gMarvellTokenSpaceGuid.PcdChip0MppBaseAddress
+  gMarvellTokenSpaceGuid.PcdChip0MppPinCount
+  gMarvellTokenSpaceGuid.PcdChip0MppSel0
+  gMarvellTokenSpaceGuid.PcdChip0MppSel1
+  gMarvellTokenSpaceGuid.PcdChip0MppSel2
+  gMarvellTokenSpaceGuid.PcdChip0MppSel3
+  gMarvellTokenSpaceGuid.PcdChip0MppSel4
+  gMarvellTokenSpaceGuid.PcdChip0MppSel5
+  gMarvellTokenSpaceGuid.PcdChip0MppSel6
+  gMarvellTokenSpaceGuid.PcdChip0MppSel7
+
+  gMarvellTokenSpaceGuid.PcdChip1MppReverseFlag
+  gMarvellTokenSpaceGuid.PcdChip1MppBaseAddress
+  gMarvellTokenSpaceGuid.PcdChip1MppPinCount
+  gMarvellTokenSpaceGuid.PcdChip1MppSel0
+  gMarvellTokenSpaceGuid.PcdChip1MppSel1
+  gMarvellTokenSpaceGuid.PcdChip1MppSel2
+  gMarvellTokenSpaceGuid.PcdChip1MppSel3
+  gMarvellTokenSpaceGuid.PcdChip1MppSel4
+  gMarvellTokenSpaceGuid.PcdChip1MppSel5
+  gMarvellTokenSpaceGuid.PcdChip1MppSel6
+  gMarvellTokenSpaceGuid.PcdChip1MppSel7
+
+  gMarvellTokenSpaceGuid.PcdChip2MppReverseFlag
+  gMarvellTokenSpaceGuid.PcdChip2MppBaseAddress
+  gMarvellTokenSpaceGuid.PcdChip2MppPinCount
+  gMarvellTokenSpaceGuid.PcdChip2MppSel0
+  gMarvellTokenSpaceGuid.PcdChip2MppSel1
+  gMarvellTokenSpaceGuid.PcdChip2MppSel2
+  gMarvellTokenSpaceGuid.PcdChip2MppSel3
+  gMarvellTokenSpaceGuid.PcdChip2MppSel4
+  gMarvellTokenSpaceGuid.PcdChip2MppSel5
+  gMarvellTokenSpaceGuid.PcdChip2MppSel6
+  gMarvellTokenSpaceGuid.PcdChip2MppSel7
+
+  gMarvellTokenSpaceGuid.PcdChip3MppReverseFlag
+  gMarvellTokenSpaceGuid.PcdChip3MppBaseAddress
+  gMarvellTokenSpaceGuid.PcdChip3MppPinCount
+  gMarvellTokenSpaceGuid.PcdChip3MppSel0
+  gMarvellTokenSpaceGuid.PcdChip3MppSel1
+  gMarvellTokenSpaceGuid.PcdChip3MppSel2
+  gMarvellTokenSpaceGuid.PcdChip3MppSel3
+  gMarvellTokenSpaceGuid.PcdChip3MppSel4
+  gMarvellTokenSpaceGuid.PcdChip3MppSel5
+  gMarvellTokenSpaceGuid.PcdChip3MppSel6
+  gMarvellTokenSpaceGuid.PcdChip3MppSel7
+
+  gMarvellTokenSpaceGuid.PcdPciESdhci
diff --git a/Platform/Marvell/Library/ParsePcdLib/ParsePcdLib.c b/Platform/Marvell/Library/ParsePcdLib/ParsePcdLib.c
new file mode 100644
index 0000000000..9a4be8ee62
--- /dev/null
+++ b/Platform/Marvell/Library/ParsePcdLib/ParsePcdLib.c
@@ -0,0 +1,228 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must Retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+#define CHAR_NULL 0x0000
+
+#include <Library/BaseLib.h>
+#include <Uefi.h>
+#include <Library/UefiLib.h>
+#include <Library/DebugLib.h>
+
+STATIC
+CHAR16
+CharToUpper (
+  IN CHAR16 Char
+  )
+{
+
+  if (Char >= L'a' && Char <= L'z') {
+    return (CHAR16) (Char - (L'a' - L'A'));
+  }
+
+  return Char;
+}
+
+STATIC
+BOOLEAN
+IsDecimalDigitChar (
+  IN CHAR16 Char
+  )
+{
+
+  return (BOOLEAN) (Char >= L'0' && Char <= L'9');
+}
+
+
+STATIC
+UINTN
+HexCharToUintn (
+  IN CHAR16 Char
+  )
+{
+  if (IsDecimalDigitChar (Char)) {
+    return Char - L'0';
+  }
+
+  return (UINTN) (10 + CharToUpper (Char) - L'A');
+}
+
+STATIC
+BOOLEAN
+IsHexDigitCharacter (
+  CHAR16 Char
+  )
+{
+
+  return (BOOLEAN) ((Char >= L'0' && Char <= L'9') || (Char >= L'A' &&
+    Char <= L'F') || (Char >= L'a' && Char <= L'f'));
+}
+
+STATIC
+UINTN
+HexStrToUintn (
+  CHAR16 *String
+  )
+{
+  UINTN Result = 0;
+
+  if (String == NULL || StrSize(String) == 0) {
+    return (UINTN)(-1);
+  }
+
+  // Ignore spaces and tabs
+  while ((*String == L' ') || (*String == L'\t')) {
+    String++;
+  }
+
+  // Ignore leading zeros after spaces
+  while (*String == L'0') {
+    String++;
+  }
+
+  if (CharToUpper (*String) != L'X') {
+    return (UINTN)(-1);
+  }
+
+  // Skip 'x'
+  String++;
+
+  while (IsHexDigitCharacter (*String)) {
+    Result <<= 4;
+    Result += HexCharToUintn (*String);
+    String++;
+  }
+
+  return (UINTN) Result;
+}
+
+STATIC
+UINTN
+DecimalStrToUintn (
+  CHAR16 *String
+  )
+{
+  UINTN Result = 0;
+
+  while (IsDecimalDigitChar (*String)) {
+    Result = 10 * Result + (*String - L'0');
+    String++;
+  }
+
+  return Result;
+}
+
+STATIC
+UINTN
+StrToUintn (
+  CHAR16 *String
+  )
+{
+  CHAR16 *Walker;
+
+  // Chop off leading spaces
+  for (Walker = String; Walker != NULL && *Walker != CHAR_NULL && *Walker == L' '; Walker++);
+
+  if (StrnCmp(Walker, L"0x", 2) == 0 || StrnCmp(Walker, L"0X", 2) == 0) {
+    return HexStrToUintn (Walker);
+  } else {
+    return DecimalStrToUintn (Walker);
+  }
+}
+
+EFI_STATUS
+ParsePcdString (
+  IN  CHAR16 *PcdString,
+  IN  UINT8  Count,
+  OUT UINTN  *ValueTable,
+  OUT CHAR16 **StrTable
+  )
+{
+  BOOLEAN ValueFlag = FALSE;
+  CHAR16 *Walker;
+  UINTN i, Tmp = 0;
+
+  if (ValueTable != NULL) {
+    ValueFlag = TRUE;
+  }
+
+  // Set pointer at the end of PCD string
+  Walker = PcdString + StrLen (PcdString);
+  for (i = 0; i < Count; i++) {
+    while ((--Walker) >= PcdString) {
+      if (*Walker == L';') {
+        // Cut off parsed chunk from PCD string by replacing ';' with
+        // null-terminator
+        *Walker = '\0';
+        if (ValueFlag) {
+          Tmp = StrToUintn ((Walker + 1));
+          if ((UINTN)(-1) == Tmp) {
+            return EFI_INVALID_PARAMETER;
+          }
+          // Entry is parsed from the end to the beginning
+          // so fill table in the same manner
+          ValueTable[Count - (i + 1)] = Tmp;
+        } else {
+          StrTable[Count - (i + 1)] = Walker + 1;
+        }
+        Walker--;
+        break;
+      }
+      if (Walker == PcdString) {
+        if (ValueFlag) {
+          Tmp = StrToUintn ((Walker));
+          if (Tmp == (UINTN)(-1)) {
+            return EFI_INVALID_PARAMETER;
+          }
+        }
+        // Last device's entry should be added to the table here.
+        // If not, return error
+        if (i != (Count - 1)) {
+          DEBUG((DEBUG_ERROR, "ParsePcdLib: Please set PCD value for every "
+            "device\n"));
+          return EFI_INVALID_PARAMETER;
+        }
+        // We parse from the end to the beginning
+        // so fill table in the same manner
+        if (ValueFlag) {
+          ValueTable[Count - (i + 1)] = Tmp;
+        } else {
+          StrTable[Count - (i + 1)] = Walker;
+        }
+        // End both loops
+        return EFI_SUCCESS;
+      }
+    }
+  }
+
+  return EFI_SUCCESS;
+}
diff --git a/Platform/Marvell/Library/ParsePcdLib/ParsePcdLib.inf b/Platform/Marvell/Library/ParsePcdLib/ParsePcdLib.inf
new file mode 100644
index 0000000000..b4db6210f7
--- /dev/null
+++ b/Platform/Marvell/Library/ParsePcdLib/ParsePcdLib.inf
@@ -0,0 +1,50 @@
+# Copyright (C) 2016 Marvell International Ltd.
+#
+# Marvell BSD License Option
+#
+# If you received this File from Marvell, you may opt to use, redistribute and/or
+# modify this File under the following licensing terms.
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright
+#   notice, this list of conditions and the following disclaimer in the
+#   documentation and/or other materials provided with the distribution.
+#
+# * Neither the name of Marvell nor the names of its contributors may be
+#   used to endorse or promote products derived from this software without
+#   specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = ParsePcdLib
+  FILE_GUID                      = 698d85a0-a952-453e-b8a4-1d6ea338a38e
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = ParsePcdLib
+
+[Packages]
+  MdePkg/MdePkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+
+[LibraryClasses]
+  ArmLib
+  DebugLib
+
+[Sources.common]
+  ParsePcdLib.c
diff --git a/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.c b/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.c
new file mode 100644
index 0000000000..95b5698909
--- /dev/null
+++ b/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.c
@@ -0,0 +1,353 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must Retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#include "UtmiPhyLib.h"
+
+typedef struct {
+  EFI_PHYSICAL_ADDRESS UtmiBaseAddr;
+  EFI_PHYSICAL_ADDRESS UsbCfgAddr;
+  EFI_PHYSICAL_ADDRESS UtmiCfgAddr;
+  UINT32 UtmiPhyPort;
+} UTMI_PHY_DATA;
+
+STATIC
+VOID
+RegSetSilent (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Data,
+  IN UINT32 Mask
+  )
+{
+  UINT32 RegData;
+
+  RegData = MmioRead32 (Addr);
+  RegData &= ~Mask;
+  RegData |= Data;
+  MmioWrite32 (Addr, RegData);
+}
+
+STATIC
+VOID
+RegSet (
+  IN EFI_PHYSICAL_ADDRESS Addr,
+  IN UINT32 Data,
+  IN UINT32 Mask
+  )
+{
+  DEBUG((DEBUG_INFO, "Write to address = %10x, data = %10x (mask = %10x)-\n",
+    Addr, Data, Mask));
+  DEBUG((DEBUG_INFO, "old value = %10x ==>\n", MmioRead32 (Addr)));
+  RegSetSilent (Addr, Data, Mask);
+  DEBUG((DEBUG_INFO, "new value %10x\n", MmioRead32 (Addr)));
+}
+
+STATIC
+VOID
+UtmiPhyPowerDown (
+  IN UINT32 UtmiIndex,
+  IN EFI_PHYSICAL_ADDRESS UtmiBaseAddr,
+  IN EFI_PHYSICAL_ADDRESS UsbCfgAddr,
+  IN EFI_PHYSICAL_ADDRESS UtmiCfgAddr,
+  IN UINT32 UtmiPhyPort
+  )
+{
+  UINT32 Mask, Data;
+
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: UTMI %d - Power down transceiver(power down Phy)\n",
+    UtmiIndex));
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: Power down PLL, and SuspendDM\n"));
+  /* Power down UTMI PHY */
+  RegSet (UtmiCfgAddr, 0x0 << UTMI_PHY_CFG_PU_OFFSET, UTMI_PHY_CFG_PU_MASK);
+  /* Config USB3 Device UTMI enable */
+  Mask = UTMI_USB_CFG_DEVICE_EN_MASK;
+
+  /*
+   * Prior to PHY init, configure mux for Device
+   * (Device can be connected to UTMI0 or to UTMI1)
+   */
+  if (UtmiPhyPort == UTMI_PHY_TO_USB_DEVICE0) {
+    Data = 0x1 << UTMI_USB_CFG_DEVICE_EN_OFFSET;
+    /* Config USB3 Device UTMI MUX */
+    Mask |= UTMI_USB_CFG_DEVICE_MUX_MASK;
+    Data |= UtmiIndex << UTMI_USB_CFG_DEVICE_MUX_OFFSET;
+  } else {
+    Data = 0x0 << UTMI_USB_CFG_DEVICE_EN_OFFSET;
+  }
+
+  /* Set Test suspendm mode */
+  Mask = UTMI_CTRL_STATUS0_SUSPENDM_MASK;
+  Data = 0x1 << UTMI_CTRL_STATUS0_SUSPENDM_OFFSET;
+  /* Enable Test UTMI select */
+  Mask |= UTMI_CTRL_STATUS0_TEST_SEL_MASK;
+  Data |= 0x1 << UTMI_CTRL_STATUS0_TEST_SEL_OFFSET;
+  RegSet (UtmiBaseAddr + UTMI_CTRL_STATUS0_REG, Data, Mask);
+
+  /* Wait for UTMI power down */
+  MicroSecondDelay (1000);
+}
+
+STATIC
+VOID
+UtmiPhyConfig (
+  IN UINT32 UtmiIndex,
+  IN EFI_PHYSICAL_ADDRESS UtmiBaseAddr,
+  IN EFI_PHYSICAL_ADDRESS UsbCfgAddr,
+  IN EFI_PHYSICAL_ADDRESS UtmiCfgAddr,
+  IN UINT32 UtmiPhyPort
+  )
+{
+  UINT32 Mask, Data;
+
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: Configure UTMI PHY %d registers\n",
+    UtmiIndex));
+  /* Reference Clock Divider Select */
+  Mask = UTMI_PLL_CTRL_REFDIV_MASK;
+  Data = 0x5 << UTMI_PLL_CTRL_REFDIV_OFFSET;
+  /* Feedback Clock Divider Select - 90 for 25Mhz */
+  Mask |= UTMI_PLL_CTRL_FBDIV_MASK;
+  Data |= 0x60 << UTMI_PLL_CTRL_FBDIV_OFFSET;
+  /* Select LPFR - 0x0 for 25Mhz/5=5Mhz */
+  Mask |= UTMI_PLL_CTRL_SEL_LPFR_MASK;
+  Data |= 0x0 << UTMI_PLL_CTRL_SEL_LPFR_OFFSET;
+  RegSet (UtmiBaseAddr + UTMI_PLL_CTRL_REG, Data, Mask);
+
+  /* Impedance Calibration Threshold Setting */
+  RegSet (UtmiBaseAddr + UTMI_CALIB_CTRL_REG,
+    0x6 << UTMI_CALIB_CTRL_IMPCAL_VTH_OFFSET,
+    UTMI_CALIB_CTRL_IMPCAL_VTH_MASK);
+
+  /* Set LS TX driver strength coarse control */
+  Mask = UTMI_TX_CH_CTRL_DRV_EN_LS_MASK;
+  Data = 0x3 << UTMI_TX_CH_CTRL_DRV_EN_LS_OFFSET;
+  /* Set LS TX driver fine adjustment */
+  Mask |= UTMI_TX_CH_CTRL_IMP_SEL_LS_MASK;
+  Data |= 0x3 << UTMI_TX_CH_CTRL_IMP_SEL_LS_OFFSET;
+  RegSet (UtmiBaseAddr + UTMI_TX_CH_CTRL_REG, Data, Mask);
+
+  /* Enable SQ */
+  Mask = UTMI_RX_CH_CTRL0_SQ_DET_MASK;
+  Data = 0x0 << UTMI_RX_CH_CTRL0_SQ_DET_OFFSET;
+  /* Enable analog squelch detect */
+  Mask |= UTMI_RX_CH_CTRL0_SQ_ANA_DTC_MASK;
+  Data |= 0x1 << UTMI_RX_CH_CTRL0_SQ_ANA_DTC_OFFSET;
+  RegSet (UtmiBaseAddr + UTMI_RX_CH_CTRL0_REG, Data, Mask);
+
+  /* Set External squelch calibration number */
+  Mask = UTMI_RX_CH_CTRL1_SQ_AMP_CAL_MASK;
+  Data = 0x1 << UTMI_RX_CH_CTRL1_SQ_AMP_CAL_OFFSET;
+  /* Enable the External squelch calibration */
+  Mask |= UTMI_RX_CH_CTRL1_SQ_AMP_CAL_EN_MASK;
+  Data |= 0x1 << UTMI_RX_CH_CTRL1_SQ_AMP_CAL_EN_OFFSET;
+  RegSet (UtmiBaseAddr + UTMI_RX_CH_CTRL1_REG, Data, Mask);
+
+  /* Set Control VDAT Reference Voltage - 0.325V */
+  Mask = UTMI_CHGDTC_CTRL_VDAT_MASK;
+  Data = 0x1 << UTMI_CHGDTC_CTRL_VDAT_OFFSET;
+  /* Set Control VSRC Reference Voltage - 0.6V */
+  Mask |= UTMI_CHGDTC_CTRL_VSRC_MASK;
+  Data |= 0x1 << UTMI_CHGDTC_CTRL_VSRC_OFFSET;
+  RegSet (UtmiBaseAddr + UTMI_CHGDTC_CTRL_REG, Data, Mask);
+}
+
+STATIC
+UINTN
+UtmiPhyPowerUp (
+  IN UINT32 UtmiIndex,
+  IN EFI_PHYSICAL_ADDRESS UtmiBaseAddr,
+  IN EFI_PHYSICAL_ADDRESS UsbCfgAddr,
+  IN EFI_PHYSICAL_ADDRESS UtmiCfgAddr,
+  IN UINT32 UtmiPhyPort
+  )
+{
+  EFI_STATUS Status;
+  UINT32 Data;
+
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: UTMI %d - Power up transceiver(Power up Phy)\n",
+    UtmiIndex));
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: exit SuspendDM\n"));
+  /* Power up UTMI PHY */
+  RegSet (UtmiCfgAddr, 0x1 << UTMI_PHY_CFG_PU_OFFSET, UTMI_PHY_CFG_PU_MASK);
+  /* Disable Test UTMI select */
+  RegSet (UtmiBaseAddr + UTMI_CTRL_STATUS0_REG,
+    0x0 << UTMI_CTRL_STATUS0_TEST_SEL_OFFSET,
+    UTMI_CTRL_STATUS0_TEST_SEL_MASK);
+
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: Wait for PLL and impedance calibration done, and PLL ready\n"));
+
+  /* Delay 10ms */
+  MicroSecondDelay (10000);
+
+  Data = MmioRead32 (UtmiBaseAddr + UTMI_CALIB_CTRL_REG);
+  if ((Data & UTMI_CALIB_CTRL_IMPCAL_DONE_MASK) == 0) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: Impedance calibration is not done\n"));
+    Status = EFI_D_ERROR;
+  }
+  if ((Data & UTMI_CALIB_CTRL_PLLCAL_DONE_MASK) == 0) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: PLL calibration is not done\n"));
+    Status = EFI_D_ERROR;
+  }
+  Data = MmioRead32 (UtmiBaseAddr + UTMI_PLL_CTRL_REG);
+  if ((Data & UTMI_PLL_CTRL_PLL_RDY_MASK) == 0) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: PLL is not ready\n"));
+    Status = EFI_D_ERROR;
+  }
+
+  return Status;
+}
+
+/*
+ * Cp110UtmiPhyInit initializes the UTMI PHY
+ * the init split in 3 parts:
+ * 1. Power down transceiver and PLL
+ * 2. UTMI PHY configure
+ * 3. Power up transceiver and PLL
+ */
+STATIC
+VOID
+Cp110UtmiPhyInit (
+  IN UINT32 UtmiPhyCount,
+  IN UTMI_PHY_DATA *UtmiData
+  )
+{
+  UINT32 i;
+
+  for (i = 0; i < UtmiPhyCount; i++) {
+    UtmiPhyPowerDown(i, UtmiData[i].UtmiBaseAddr,
+      UtmiData[i].UsbCfgAddr, UtmiData[i].UtmiCfgAddr,
+      UtmiData[i].UtmiPhyPort);
+  }
+
+  /* Power down PLL */
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: PHY power down PLL\n"));
+  RegSet (UtmiData[0].UsbCfgAddr, 0x0 << UTMI_USB_CFG_PLL_OFFSET,
+    UTMI_USB_CFG_PLL_MASK);
+
+  for (i = 0; i < UtmiPhyCount; i++) {
+    UtmiPhyConfig(i, UtmiData[i].UtmiBaseAddr,
+      UtmiData[i].UsbCfgAddr, UtmiData[i].UtmiCfgAddr,
+      UtmiData[i].UtmiPhyPort);
+  }
+
+  for (i = 0; i < UtmiPhyCount; i++) {
+    if (EFI_ERROR(UtmiPhyPowerUp(i, UtmiData[i].UtmiBaseAddr,
+        UtmiData[i].UsbCfgAddr, UtmiData[i].UtmiCfgAddr,
+        UtmiData[i].UtmiPhyPort))) {
+      DEBUG((DEBUG_ERROR, "UtmiPhy: Failed to initialize UTMI PHY %d\n", i));
+      continue;
+    }
+    DEBUG((DEBUG_ERROR, "UTMI PHY %d initialized to ", i));
+
+    if (UtmiData[i].UtmiPhyPort == UTMI_PHY_TO_USB_DEVICE0)
+      DEBUG((DEBUG_ERROR, "USB Device\n"));
+    else
+      DEBUG((DEBUG_ERROR, "USB Host%d\n", UtmiData[i].UtmiPhyPort));
+  }
+
+  /* Power up PLL */
+  DEBUG((DEBUG_INFO, "UtmiPhy: stage: PHY power up PLL\n"));
+  RegSet (UtmiData[0].UsbCfgAddr, 0x1 << UTMI_USB_CFG_PLL_OFFSET,
+    UTMI_USB_CFG_PLL_MASK);
+}
+
+EFI_STATUS
+UtmiPhyInit (
+  VOID
+  )
+{
+  EFI_STATUS Status;
+  UTMI_PHY_DATA UtmiData[PcdGet32 (PcdUtmiPhyCount)];
+  EFI_PHYSICAL_ADDRESS RegUtmiUnit[PcdGet32 (PcdUtmiPhyCount)];
+  EFI_PHYSICAL_ADDRESS RegUsbCfg[PcdGet32 (PcdUtmiPhyCount)];
+  EFI_PHYSICAL_ADDRESS RegUtmiCfg[PcdGet32 (PcdUtmiPhyCount)];
+  UINTN UtmiPort[PcdGet32 (PcdUtmiPhyCount)];
+  UINTN i, Count;
+
+  Count = PcdGet32 (PcdUtmiPhyCount);
+  if (Count == 0) {
+    /* No UTMI PHY on platform */
+    return EFI_SUCCESS;
+  }
+
+  DEBUG((DEBUG_INFO, "UtmiPhy: Initialize USB UTMI PHYs\n"));
+  /* Parse UtmiPhy PCDs */
+  Status = ParsePcdString ((CHAR16 *) PcdGetPtr (PcdUtmiPhyRegUtmiUnit),
+    Count, RegUtmiUnit, NULL);
+  if (EFI_ERROR(Status)) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: Wrong PcdUtmiPhyRegUtmiUnit format\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = ParsePcdString ((CHAR16 *) PcdGetPtr (PcdUtmiPhyRegUsbCfg),
+    Count, RegUsbCfg, NULL);
+  if (EFI_ERROR(Status)) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: Wrong PcdUtmiPhyRegUsbCfg format\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = ParsePcdString ((CHAR16 *) PcdGetPtr (PcdUtmiPhyRegUtmiCfg),
+    Count, RegUtmiCfg, NULL);
+  if (EFI_ERROR(Status)) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: Wrong PcdUtmiPhyRegUtmiCfg format\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  Status = ParsePcdString ((CHAR16 *) PcdGetPtr (PcdUtmiPhyUtmiPort),
+    Count, UtmiPort, NULL);
+  if (EFI_ERROR(Status)) {
+    DEBUG((DEBUG_ERROR, "UtmiPhy: Wrong PcdUtmiPhyUtmiPort format\n"));
+    return EFI_INVALID_PARAMETER;
+  }
+
+  for (i = 0 ; i < Count ; i++) {
+    /* Get base address of UTMI phy */
+    UtmiData[i].UtmiBaseAddr = RegUtmiUnit[i];
+
+    /* Get usb config address */
+    UtmiData[i].UsbCfgAddr = RegUsbCfg[i];
+
+    /* Get UTMI config address */
+    UtmiData[i].UtmiCfgAddr = RegUtmiCfg[i];
+
+    /*
+     * Get the usb port number, which will be used to check if
+     * the utmi connected to host or device
+     */
+    UtmiData[i].UtmiPhyPort = UtmiPort[i];
+  }
+
+  /* Currently only Cp110 is supported */
+  Cp110UtmiPhyInit (Count, UtmiData);
+
+  return EFI_SUCCESS;
+}
diff --git a/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.h b/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.h
new file mode 100644
index 0000000000..f9b49339d0
--- /dev/null
+++ b/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.h
@@ -0,0 +1,110 @@
+/********************************************************************************
+Copyright (C) 2016 Marvell International Ltd.
+
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or
+modify this File under the following licensing terms.
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+* Neither the name of Marvell nor the names of its contributors may be
+  used to endorse or promote products derived from this software without
+  specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*******************************************************************************/
+
+#ifndef __UTMIPHY_H__
+#define __UTMIPHY_H__
+
+#include <Library/ArmLib.h>
+#include <Library/ArmPlatformLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PcdLib.h>
+#include <Library/MemoryAllocationLib.h>
+#include <Library/IoLib.h>
+#include <Library/TimerLib.h>
+#include <Library/ParsePcdLib.h>
+
+#define UTMI_USB_CFG_DEVICE_EN_OFFSET             0
+#define UTMI_USB_CFG_DEVICE_EN_MASK               (0x1 << UTMI_USB_CFG_DEVICE_EN_OFFSET)
+#define UTMI_USB_CFG_DEVICE_MUX_OFFSET            1
+#define UTMI_USB_CFG_DEVICE_MUX_MASK              (0x1 << UTMI_USB_CFG_DEVICE_MUX_OFFSET)
+#define UTMI_USB_CFG_PLL_OFFSET                   25
+#define UTMI_USB_CFG_PLL_MASK                     (0x1 << UTMI_USB_CFG_PLL_OFFSET)
+
+#define UTMI_PHY_CFG_PU_OFFSET                    5
+#define UTMI_PHY_CFG_PU_MASK                      (0x1 << UTMI_PHY_CFG_PU_OFFSET)
+
+#define UTMI_PLL_CTRL_REG                         0x0
+#define UTMI_PLL_CTRL_REFDIV_OFFSET               0
+#define UTMI_PLL_CTRL_REFDIV_MASK                 (0x7f << UTMI_PLL_CTRL_REFDIV_OFFSET)
+#define UTMI_PLL_CTRL_FBDIV_OFFSET                16
+#define UTMI_PLL_CTRL_FBDIV_MASK                  (0x1FF << UTMI_PLL_CTRL_FBDIV_OFFSET)
+#define UTMI_PLL_CTRL_SEL_LPFR_OFFSET             28
+#define UTMI_PLL_CTRL_SEL_LPFR_MASK               (0x3 << UTMI_PLL_CTRL_SEL_LPFR_OFFSET)
+#define UTMI_PLL_CTRL_PLL_RDY_OFFSET              31
+#define UTMI_PLL_CTRL_PLL_RDY_MASK                (0x1 << UTMI_PLL_CTRL_PLL_RDY_OFFSET)
+
+#define UTMI_CALIB_CTRL_REG                       0x8
+#define UTMI_CALIB_CTRL_IMPCAL_VTH_OFFSET         8
+#define UTMI_CALIB_CTRL_IMPCAL_VTH_MASK           (0x7 << UTMI_CALIB_CTRL_IMPCAL_VTH_OFFSET)
+#define UTMI_CALIB_CTRL_IMPCAL_DONE_OFFSET        23
+#define UTMI_CALIB_CTRL_IMPCAL_DONE_MASK          (0x1 << UTMI_CALIB_CTRL_IMPCAL_DONE_OFFSET)
+#define UTMI_CALIB_CTRL_PLLCAL_DONE_OFFSET        31
+#define UTMI_CALIB_CTRL_PLLCAL_DONE_MASK          (0x1 << UTMI_CALIB_CTRL_PLLCAL_DONE_OFFSET)
+
+#define UTMI_TX_CH_CTRL_REG                       0xC
+#define UTMI_TX_CH_CTRL_DRV_EN_LS_OFFSET          12
+#define UTMI_TX_CH_CTRL_DRV_EN_LS_MASK            (0xf << UTMI_TX_CH_CTRL_DRV_EN_LS_OFFSET)
+#define UTMI_TX_CH_CTRL_IMP_SEL_LS_OFFSET         16
+#define UTMI_TX_CH_CTRL_IMP_SEL_LS_MASK           (0xf << UTMI_TX_CH_CTRL_IMP_SEL_LS_OFFSET)
+
+#define UTMI_RX_CH_CTRL0_REG                      0x14
+#define UTMI_RX_CH_CTRL0_SQ_DET_OFFSET            15
+#define UTMI_RX_CH_CTRL0_SQ_DET_MASK              (0x1 << UTMI_RX_CH_CTRL0_SQ_DET_OFFSET)
+#define UTMI_RX_CH_CTRL0_SQ_ANA_DTC_OFFSET        28
+#define UTMI_RX_CH_CTRL0_SQ_ANA_DTC_MASK          (0x1 << UTMI_RX_CH_CTRL0_SQ_ANA_DTC_OFFSET)
+
+#define UTMI_RX_CH_CTRL1_REG                      0x18
+#define UTMI_RX_CH_CTRL1_SQ_AMP_CAL_OFFSET        0
+#define UTMI_RX_CH_CTRL1_SQ_AMP_CAL_MASK          (0x3 << UTMI_RX_CH_CTRL1_SQ_AMP_CAL_OFFSET)
+#define UTMI_RX_CH_CTRL1_SQ_AMP_CAL_EN_OFFSET     3
+#define UTMI_RX_CH_CTRL1_SQ_AMP_CAL_EN_MASK       (0x1 << UTMI_RX_CH_CTRL1_SQ_AMP_CAL_EN_OFFSET)
+
+#define UTMI_CTRL_STATUS0_REG                     0x24
+#define UTMI_CTRL_STATUS0_SUSPENDM_OFFSET         22
+#define UTMI_CTRL_STATUS0_SUSPENDM_MASK           (0x1 << UTMI_CTRL_STATUS0_SUSPENDM_OFFSET)
+#define UTMI_CTRL_STATUS0_TEST_SEL_OFFSET         25
+#define UTMI_CTRL_STATUS0_TEST_SEL_MASK           (0x1 << UTMI_CTRL_STATUS0_TEST_SEL_OFFSET)
+
+#define UTMI_CHGDTC_CTRL_REG                      0x38
+#define UTMI_CHGDTC_CTRL_VDAT_OFFSET              8
+#define UTMI_CHGDTC_CTRL_VDAT_MASK                (0x3 << UTMI_CHGDTC_CTRL_VDAT_OFFSET)
+#define UTMI_CHGDTC_CTRL_VSRC_OFFSET              10
+#define UTMI_CHGDTC_CTRL_VSRC_MASK                (0x3 << UTMI_CHGDTC_CTRL_VSRC_OFFSET)
+
+#define UTMI_PHY_TO_USB_HOST0                     0
+#define UTMI_PHY_TO_USB_HOST1                     1
+#define UTMI_PHY_TO_USB_DEVICE0                   2
+#define UTMI_PHY_INVALID                          0xff
+
+#endif
diff --git a/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.inf b/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.inf
new file mode 100644
index 0000000000..f1e57f46f9
--- /dev/null
+++ b/Platform/Marvell/Library/UtmiPhyLib/UtmiPhyLib.inf
@@ -0,0 +1,64 @@
+# Copyright (C) 2016 Marvell International Ltd.
+#
+# Marvell BSD License Option
+#
+# If you received this File from Marvell, you may opt to use, redistribute and/or
+# modify this File under the following licensing terms.
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright
+#   notice, this list of conditions and the following disclaimer in the
+#   documentation and/or other materials provided with the distribution.
+#
+# * Neither the name of Marvell nor the names of its contributors may be
+#   used to endorse or promote products derived from this software without
+#   specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+
+[Defines]
+  INF_VERSION                    = 0x00010019
+  BASE_NAME                      = MarvellUtmiPhyLib
+  FILE_GUID                      = e9adaac2-0443-4921-9367-5d575c3c91bc
+  MODULE_TYPE                    = BASE
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = UtmiPhyLib
+
+[Packages]
+  ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
+  MdeModulePkg/MdeModulePkg.dec
+  MdePkg/MdePkg.dec
+  Platform/Marvell/Marvell.dec
+
+[LibraryClasses]
+  ArmLib
+  DebugLib
+  IoLib
+  MemoryAllocationLib
+  ParsePcdLib
+  PcdLib
+
+[Sources.common]
+  UtmiPhyLib.c
+
+[FixedPcd]
+  gMarvellTokenSpaceGuid.PcdUtmiPhyCount
+  gMarvellTokenSpaceGuid.PcdUtmiPhyRegUsbCfg
+  gMarvellTokenSpaceGuid.PcdUtmiPhyRegUtmiCfg
+  gMarvellTokenSpaceGuid.PcdUtmiPhyRegUtmiUnit
+  gMarvellTokenSpaceGuid.PcdUtmiPhyUtmiPort