Update AMD F14 Agesa to support Rev C0 cpus

This change updates the AMD Agesa code to support the Family 14
rev C0 cpus.  It also fixes (again) a ton of warnings, although
not all of them are gone.  The warning fixes affect code in the
Family 12 tree as well, so there are some small changes therein.
This code has been tested on a Persimmon and passes Abuild.
This is the first (and largest) of a number of commits to complete
the upgrade.

Change-Id: Id28d9bf7931f8baa2a602f6bb096a5a465ccd20d
Signed-off-by: Frank Vibrans <frank.vibrans@amd.com>
Signed-off-by: efdesign98 <efdesign98@gmail.com>
Reviewed-on: http://review.coreboot.org/131
Tested-by: build bot (Jenkins)
Reviewed-by: Patrick Georgi <patrick@georgi-clan.de>

5 files changed, 905 insertions, 592 deletions

diff --git a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlib.c b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlib.c
index b28e1a9189..a1fd4190ec 100644
--- a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlib.c
+++ b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlib.c
@@ -74,6 +74,8 @@
  */
 
 extern UINT8  AlibSsdt[];
+extern AGESA_STATUS PcieFmAlibBuildAcpiTable (VOID *AlibSsdtPtr, AMD_CONFIG_PARAMS *StdHeader);
+;
 
 /*----------------------------------------------------------------------------------------
  *                  T Y P E D E F S     A N D     S T R U C T U  R E S
@@ -88,7 +90,15 @@ extern UINT8  AlibSsdt[];
 
 VOID
 STATIC
-PcieAlibSetPortGenCapabilityCallback (
+PcieAlibSetPortMaxSpeedCallback (
+  IN       PCIe_ENGINE_CONFIG    *Engine,
+  IN OUT   VOID                  *Buffer,
+  IN       PCIe_PLATFORM_CONFIG  *Pcie
+  );
+
+VOID
+STATIC
+PcieAlibSetPortOverrideSpeedCallback (
   IN       PCIe_ENGINE_CONFIG    *Engine,
   IN OUT   VOID                  *Buffer,
   IN       PCIe_PLATFORM_CONFIG  *Pcie
@@ -177,13 +187,14 @@ PcieAlibBuildAcpiTable (
   // Copy template to buffer
   LibAmdMemCopy (AlibSsdtBuffer, &AlibSsdt[0], AlibSsdtlength, StdHeader);
   // Set PCI MMIO configuration
-  AmlObjName = '10DA';
+//  AmlObjName = '10DA';
+  AmlObjName = Int32FromChar ('1', '0', 'D', 'A');
   AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
   if (AmlObjPtr != NULL) {
-    UINT64  MsrRegister;
-    LibAmdMsrRead (MSR_MMIO_Cfg_Base, &MsrRegister, StdHeader);
-    if ((MsrRegister & BIT0) != 0 && (MsrRegister & 0xFFFFFFFF00000000) == 0) {
-      *(UINT32*)((UINT8*)AmlObjPtr + 5) = (UINT32)(MsrRegister & 0xFFFFF00000);
+    UINT64  MsrReg;
+    LibAmdMsrRead (MSR_MMIO_Cfg_Base, &MsrReg, StdHeader);
+    if ((MsrReg & BIT0) != 0 && (MsrReg & 0xFFFFFFFF00000000) == 0) {
+      *(UINT32*)((UINT8*)AmlObjPtr + 5) = (UINT32)(MsrReg & 0xFFFFF00000);
     } else {
       Status = AGESA_ERROR;
     }
@@ -193,7 +204,8 @@ PcieAlibBuildAcpiTable (
   // Set voltage configuration
   PpFuseArray = GnbLocateHeapBuffer (AMD_PP_FUSE_TABLE_HANDLE, StdHeader);
   if (PpFuseArray != NULL) {
-    AmlObjName = '30DA';
+//    AmlObjName = '30DA';
+    AmlObjName = Int32FromChar ('3', '0', 'D', 'A');
     AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
     ASSERT (AmlObjPtr != NULL);
     if (AmlObjPtr != NULL) {
@@ -222,7 +234,8 @@ PcieAlibBuildAcpiTable (
       BootUpVidIndex = (UINT8) Index;
     }
   }
-  AmlObjName = '40DA';
+//  AmlObjName = '40DA';
+  AmlObjName = Int32FromChar ('4', '0', 'D', 'A');
   AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
   ASSERT (AmlObjPtr != NULL);
   if (AmlObjPtr != NULL) {
@@ -230,7 +243,8 @@ PcieAlibBuildAcpiTable (
   } else {
     Status = AGESA_ERROR;
   }
-  AmlObjName = '50DA';
+//  AmlObjName = '50DA';
+  AmlObjName = Int32FromChar ('5', '0', 'D', 'A');
   AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
   ASSERT (AmlObjPtr != NULL);
   if (AmlObjPtr != NULL) {
@@ -240,7 +254,8 @@ PcieAlibBuildAcpiTable (
   }
   // Set PCIe configuration
   if (PcieLocateConfigurationData (StdHeader, &Pcie) == AGESA_SUCCESS) {
-    AmlObjName = '20DA';
+//    AmlObjName = '20DA';
+  AmlObjName = Int32FromChar ('2', '0', 'D', 'A');
     AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
     ASSERT (AmlObjPtr != NULL);
     if (AmlObjPtr != NULL) {
@@ -248,20 +263,36 @@ PcieAlibBuildAcpiTable (
     } else {
       Status = AGESA_ERROR;
     }
-    AmlObjName = '60DA';
+//    AmlObjName = '60DA';
+    AmlObjName = Int32FromChar ('6', '0', 'D', 'A');
+    AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
+    ASSERT (AmlObjPtr != NULL);
+    if (AmlObjPtr != NULL) {
+      PcieConfigRunProcForAllEngines (
+        DESCRIPTOR_ALLOCATED | DESCRIPTOR_PCIE_ENGINE,
+        PcieAlibSetPortMaxSpeedCallback,
+        (UINT8*)((UINT8*)AmlObjPtr + 7),
+        Pcie
+        );
+    } else {
+      Status = AGESA_ERROR;
+    }
+//    AmlObjName = '80DA';
+    AmlObjName = Int32FromChar ('6', '0', 'D', 'A');
     AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
     ASSERT (AmlObjPtr != NULL);
     if (AmlObjPtr != NULL) {
       PcieConfigRunProcForAllEngines (
         DESCRIPTOR_ALLOCATED | DESCRIPTOR_PCIE_ENGINE,
-        PcieAlibSetPortGenCapabilityCallback,
+        PcieAlibSetPortOverrideSpeedCallback,
         (UINT8*)((UINT8*)AmlObjPtr + 7),
         Pcie
         );
     } else {
       Status = AGESA_ERROR;
     }
-    AmlObjName = '70DA';
+//    AmlObjName = '70DA';
+    AmlObjName = Int32FromChar ('6', '0', 'D', 'A');
     AmlObjPtr = GnbLibFind (AlibSsdtBuffer, AlibSsdtlength, (UINT8*) &AmlObjName, sizeof (AmlObjName));
     ASSERT (AmlObjPtr != NULL);
     if (AmlObjPtr != NULL) {
@@ -278,7 +309,10 @@ PcieAlibBuildAcpiTable (
     ASSERT (FALSE);
     Status = AGESA_ERROR;
   }
-  if (Status == AGESA_ERROR) {
+  if (Status == AGESA_SUCCESS) {
+    Status = PcieFmAlibBuildAcpiTable (AlibSsdtBuffer, StdHeader);
+  }
+  if (Status != AGESA_SUCCESS) {
     //Shrink table length to size of the header
     ((ACPI_TABLE_HEADER*) AlibSsdtBuffer)->TableLength = sizeof (ACPI_TABLE_HEADER);
   }
@@ -288,7 +322,7 @@ PcieAlibBuildAcpiTable (
 
 /*----------------------------------------------------------------------------------------*/
 /**
- * Callback to init max port Gen capability
+ * Callback to init max port speed capability
  *
  *
  *
@@ -301,16 +335,47 @@ PcieAlibBuildAcpiTable (
 
 VOID
 STATIC
-PcieAlibSetPortGenCapabilityCallback (
+PcieAlibSetPortMaxSpeedCallback (
   IN       PCIe_ENGINE_CONFIG    *Engine,
   IN OUT   VOID                  *Buffer,
   IN       PCIe_PLATFORM_CONFIG  *Pcie
   )
 {
-  UINT8   *PsppMaxPortCapbilityArray;
-  PsppMaxPortCapbilityArray = (UINT8*) Buffer;
+  UINT8   *PsppMaxPortSpeedPackage;
+  PsppMaxPortSpeedPackage = (UINT8*) Buffer;
   if (Engine->Type.Port.PortData.LinkHotplug != HotplugDisabled || PcieConfigCheckPortStatus (Engine, INIT_STATUS_PCIE_TRAINING_SUCCESS)) {
-    PsppMaxPortCapbilityArray[(Engine->Type.Port.Address.Address.Device - 2) * 2 + 1] = (UINT8) PcieFmGetLinkSpeedCap (PCIE_PORT_GEN_CAP_MAX, Engine, Pcie) + 1;
+    PsppMaxPortSpeedPackage[(Engine->Type.Port.Address.Address.Device - 2) * 2 + 1] = (UINT8) PcieFmGetLinkSpeedCap (PCIE_PORT_GEN_CAP_MAX, Engine, Pcie);
+  }
+}
+
+/*----------------------------------------------------------------------------------------*/
+/**
+ * Callback to init max port speed capability
+ *
+ *
+ *
+ *
+ * @param[in]       Engine          Pointer to engine config descriptor
+ * @param[in, out]  Buffer          Not used
+ * @param[in]       Pcie            Pointer to global PCIe configuration
+ *
+ */
+
+VOID
+STATIC
+PcieAlibSetPortOverrideSpeedCallback (
+  IN       PCIe_ENGINE_CONFIG    *Engine,
+  IN OUT   VOID                  *Buffer,
+  IN       PCIe_PLATFORM_CONFIG  *Pcie
+  )
+{
+  UINT8   *PsppOverridePortSpeedPackage;
+  PsppOverridePortSpeedPackage = (UINT8*) Buffer;
+  if (Engine->Type.Port.PortData.LinkHotplug != HotplugDisabled || PcieConfigCheckPortStatus (Engine, INIT_STATUS_PCIE_TRAINING_SUCCESS)) {
+    PsppOverridePortSpeedPackage[(Engine->Type.Port.Address.Address.Device - 2) * 2 + 1] = Engine->Type.Port.PortData.MiscControls.LinkSafeMode;
+  }
+  if (Engine->Type.Port.PortData.LinkHotplug == HotplugBasic && !PcieConfigCheckPortStatus (Engine, INIT_STATUS_PCIE_TRAINING_SUCCESS)) {
+    PsppOverridePortSpeedPackage[(Engine->Type.Port.Address.Address.Device - 2) * 2 + 1] = PcieGen1;
   }
 }
 
@@ -344,7 +409,8 @@ PcieAlibSetPortInfoCallback (
   PortInfoPackage->PortInfo[PortIndex].StartCoreLane = (UINT8) Engine->Type.Port.StartCoreLane;
   PortInfoPackage->PortInfo[PortIndex].EndCoreLane = (UINT8) Engine->Type.Port.EndCoreLane;
   PortInfoPackage->PortInfo[PortIndex].PortId = Engine->Type.Port.PortId;
-  PortInfoPackage->PortInfo[PortIndex].WrapperId = 0x0130 | (PcieEngineGetParentWrapper (Engine)->WrapId);
+//  PortInfoPackage->PortInfo[PortIndex].WrapperId = 0x0130 | (PcieEngineGetParentWrapper (Engine)->WrapId);
+  PortInfoPackage->PortInfo[PortIndex].WrapperId = 0x0130u | (PcieEngineGetParentWrapper (Engine)->WrapId);
   PortInfoPackage->PortInfo[PortIndex].LinkHotplug = Engine->Type.Port.PortData.LinkHotplug;
   PortInfoPackage->PortInfo[PortIndex].MaxSpeedCap = (UINT8) PcieFmGetLinkSpeedCap (PCIE_PORT_GEN_CAP_MAX, Engine, Pcie);
 }
diff --git a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibConfig.esl b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibConfig.esl
index e595c28036..5150ee1d22 100644
--- a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibConfig.esl
+++ b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibConfig.esl
@@ -65,4 +65,39 @@
 #define DEF_PSPP_STATE_AC             0
 #define DEF_PSPP_STATE_DC             1
 
+#define DEF_TRAINING_STATE_COMPLETE             0
+#define DEF_TRAINING_STATE_DETECT_PRESENCE      1
+#define DEF_TRAINING_STATE_PRESENCE_DETECTED    2
+#define DEF_TRAINING_GEN2_WORKAROUND            3
+#define DEF_TRAINING_STATE_NOT_PRESENT          4
+#define DEF_TRAINING_DEVICE_PRESENT             5
+#define DEF_TRAINING_STATE_RELEASE_TRAINING     6
+#define DEF_TRAINING_STATE_REQUEST_RESET        7
+#define DEF_TRAINING_STATE_EXIT                 8
+
+#define DEF_LINK_SPEED_GEN1                     1
+#define DEF_LINK_SPEED_GEN2                     2
+
+#define DEF_HOTPLUG_STATUS_DEVICE_NOT_PRESENT   0
+#define DEF_HOTPLUG_STATUS_DEVICE_PRESENT       1
+
+#define DEF_PORT_NOT_ALLOCATED                  0
+#define DEF_PORT_ALLOCATED                      1
+
+#define DEF_PCIE_LANE_POWERON                   1
+#define DEF_PCIE_LANE_POWEROFF                  0
+#define DEF_PCIE_LANE_POWEROFFUNUSED            2
+
+#define DEF_SCARTCH_PSPP_START_OFFSET           0
+#define DEF_SCARTCH_PSPP_POLICY_OFFSET          1
+#define DEF_SCARTCH_PSPP_ACDC_OFFSET            5
+#define DEF_SCARTCH_PSPP_ACDC_OVR_OFFSET        6
+#define DEF_SCARTCH_PSPP_REQ_OFFSET             16
+
+#define DEF_LINKWIDTH_ACTIVE                    0
+#define DEF_LINKWIDTH_MAX_PHY                   1
+
+#define TRUE                                    1
+#define FALSE                                   0
+
 #endif
diff --git a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibCore.esl b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibCore.esl
index 7b785a80e8..ab67b9e02e 100644
--- a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibCore.esl
+++ b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibCore.esl
@@ -78,6 +78,7 @@
       Buffer () {0,0,0,0,0,0,0,0,0,0},    //dev6
       Buffer () {0,0,0,0,0,0,0,0,0,0},    //dev7
       Buffer () {0,0,0,0,0,0,0,0,0,0},    //dev8
+      Buffer () {0,0,0,0,0,0,0,0,0,0},    //dev9
     }
   )
 
@@ -86,6 +87,9 @@
     varPortInfo
   )
 
+
+  Name (varStringBuffer, Buffer (256) {})
+
   /*----------------------------------------------------------------------------------------*/
   /**
    *  Master control method
@@ -128,20 +132,12 @@
 
   /*----------------------------------------------------------------------------------------*/
   /**
-   *  Read PCI config register
-   *
-   *  Arg0 - Port Index
-   *
-   */
-
-  /*----------------------------------------------------------------------------------------*/
-  /**
    *  Read PCI config register through MMIO
    *
    *  Arg0 - PCI address Bus/device/func
    *  Arg1 - Register offset
    */
-  Method (procPciDwordRead, 2, NotSerialized) {
+  Method (procPciDwordRead, 2, Serialized) {
     Add (varPcieBase, ShiftLeft (Arg0, 12), Local0)
     Add (Arg1, Local0, Local0)
     OperationRegion(varOperationRegionMmio, SystemMemory, Local0, 0x4)
@@ -159,7 +155,7 @@
    *  Arg1 - Register offset
    *  Arg2 - Value
    */
-  Method (procPciDwordWrite, 3, NotSerialized) {
+  Method (procPciDwordWrite, 3, Serialized) {
     Add (varPcieBase, ShiftLeft (Arg0, 12), Local0)
     Add (Arg1, Local0, Local0)
     OperationRegion(varOperationRegionMmio, SystemMemory, Local0, 0x4)
@@ -178,7 +174,7 @@
    *  Arg2 - AND mask
    *  Arg3 - OR mask
    */
-  Method (procPciDwordRMW, 4, NotSerialized) {
+  Method (procPciDwordRMW, 4, Serialized) {
     Store (procPciDwordRead (Arg0, Arg1), Local0)
     Or (And (Local0, Arg2), Arg3, Local0)
     procPciDwordWrite (Arg0, Arg1, Local0)
@@ -310,14 +306,15 @@
     Store (1, Local0)
     while (LEqual (Local0, 1)) {
       Store (And (procPciDwordRead (Arg0, Local1), 0xFF), Local1)
-      if (LNotEqual (Local1, 0)) {
+      if (LEqual (Local1, 0)) {
+        break
+      }
         if (LEqual (And (procPciDwordRead (Arg0, Local1), 0xFF), Arg1)) {
         	Store (0, Local0)
         } else {
         	Increment (Local1)
         }
       }
-    }
     return (Local1)
   }
 
@@ -328,7 +325,7 @@
    *  Arg0 	- Aspm
    *  Arg1 	- 0: Read, 1: Write
    */
-  Method (procPcieSbAspmControl, 2, NotSerialized) {
+  Method (procPcieSbAspmControl, 2, Serialized) {
 	  // Create an opregion for PM IO Registers
 	  OperationRegion (PMIO, SystemIO, 0xCD6, 0x2)
 	  Field (PMIO, ByteAcc, NoLock, Preserve)
@@ -359,15 +356,5 @@
       Or (And (Local0, 0xfffffffc), Arg0, Local0)
       Store (Local0, ABDA)
     }
-    
-  }
-
-#ifdef  ALIB_DEBUG
-  Name (ABUF, Buffer (256) {})
-  Name (AFUN, 0xff)
-  Method (ADBG, 0, Serialized) {
-    ALIB (AFUN, ABUF);
   }
-  Alias (procPciDwordRead, AXPR)
-#endif
 
diff --git a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibHotplug.esl b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibHotplug.esl
index fead211aed..5fa1a9774e 100644
--- a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibHotplug.esl
+++ b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibHotplug.esl
@@ -12,37 +12,45 @@
  *
  */
 /*
- *****************************************************************************
- *
- * Copyright (c) 2011, Advanced Micro Devices, Inc.
- * All rights reserved.
- * 
- * 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 Advanced Micro Devices, Inc. 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 ADVANCED MICRO DEVICES, INC. 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.
- * 
- * ***************************************************************************
- *
- */
+*****************************************************************************
+*
+* Copyright (c) 2011, Advanced Micro Devices, Inc.
+* All rights reserved.
+*
+* 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 Advanced Micro Devices, Inc. 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 ADVANCED MICRO DEVICES, INC. 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.
+*
+****************************************************************************
+*
+*/
 
+   External(\_SB.ALIC,  MethodObj)
+
+  Name (varStartPhyLane, 0)
+  Name (varEndPhyLane, 0)
+  Name (varStartCoreLane, 0)
+  Name (varEndCoreLane, 0)
+  Name (varWrapperId, 0)
+  Name (varPortId, 0)
 
   /*----------------------------------------------------------------------------------------*/
   /**
@@ -50,12 +58,12 @@
    *
    *  Arg0 - Data Buffer
    */
-  Method (procPcieSetBusWidth, 1, Serialized) {
+  Method (procPcieSetBusWidth, 1, NotSerialized) {
     Store (Buffer (256) {}, Local7)
     CreateWordField (Local7, 0x0, varReturnBufferLength)
     CreateWordField (Local7, 0x2, varReturnBusWidth)
     CreateByteField (Arg0, 0x2, varArgBusWidth)
-    //@todo deternime correct lane bitmap (check for reversal) gate/ungate unused lanes
+    //deternime correct lane bitmap (check for reversal) gate/ungate unused lanes
     Store (3, varReturnBufferLength)
     Store (varArgBusWidth, varReturnBusWidth)
     return (Local7)
@@ -66,202 +74,310 @@
   /**
    *  PCIe port hotplug
    *
-   *  Arg0 - Data Buffer
-   *  Local7 - Return buffer
+   *  Arg0   - Data Buffer
+   *  Retval - Return buffer
    */
   Method (procPciePortHotplug, 1, Serialized) {
     Store ("PciePortHotplug Enter", Debug)
-    Store (Buffer (256) {}, Local7)
-    CreateWordField (Local7, 0x0, varReturnBufferLength)
-    CreateByteField (Local7, 0x2, varReturnStatus)
-    CreateByteField (Local7, 0x3, varReturnDeviceStatus)
-    CreateWordField (Arg0, 0x2, varPortBdf)
-    CreateByteField (Arg0, 0x4, varHotplugState)
-    Subtract (ShiftRight (varPortBdf, 3), 2, Local1);
-    if (LEqual(varHotplugState, 1)) {
+    Store (DerefOf (Index (Arg0, 4)), varHotplugStateLocal0)
+    Store (DerefOf (Index (Arg0, 2)), varPortIndexLocal1)
+
+    Subtract (ShiftRight (varPortBdfLocal1, 3), 2, varPortIndexLocal1)
+    if (LEqual(varHotplugStateLocal0, 1)) {
       // Enable port
-      Store (procPciePortEnable (Local1), varHotplugState);
+      Store (DEF_TRAINING_STATE_RELEASE_TRAINING, Local2)
     } else {
       // Disable port
-      Store (procPciePortDisable (Local1), varHotplugState);
+      Store (DEF_TRAINING_STATE_NOT_PRESENT, Local2)
     }
+
+    Store (procPciePortTraining (varPortIndexLocal1, Local2), varHotplugStateLocal0)
+
+    Store (Buffer (10) {}, Local7)
+    CreateWordField (Local7, 0x0, varReturnBufferLength)
+    CreateByteField (Local7, 0x2, varReturnStatus)
+    CreateByteField (Local7, 0x3, varReturnDeviceStatus)
     Store (0x4, varReturnBufferLength)
     Store (0x0, varReturnStatus)
-    Store (varHotplugState, varReturnDeviceStatus)
+    Store (varHotplugStateLocal0, varReturnDeviceStatus)
     Store ("PciePortHotplug Exit", Debug)
     return (Local7)
   }
 
-    
+  Name (varSpeedRequest, Buffer (10) {0,0,0,0,0,0,0,0,0,0})
+
   /*----------------------------------------------------------------------------------------*/
   /**
-   *  Enable PCIe port 
-   *  
-   *  1) Ungate lanes
-   *  2) Enable Lanes
-   *  3) Train port
-   *  4) Disable unused lanes
-   *  5) Gate unused lanes
+   *  Train PCIe port
+   *
    *
    *  Arg0 	- Port Index
-   *  
+   *  Arg1  - Initial state
    */
-  Method (procPciePortEnable, 1, NotSerialized) {
-    Store ("PciePortEnable Enter", Debug)
-    Name (varLinkIsLinkReversed, 0)
+  Method (procPciePortTraining, 2, Serialized) {
+    Store ("PciePortTraining Enter", Debug)
+    Store (DEF_HOTPLUG_STATUS_DEVICE_NOT_PRESENT, varResultLocal4)
     Store (procPcieGetPortInfo (Arg0), Local7)
-    CreateByteField (Local7, DEF_OFFSET_LINK_HOTPLUG, varHotplugType)
-    if (LNotEqual (varHotplugType, DEF_BASIC_HOTPLUG)) {
+    // Check if port supports basic hotplug
+    Store (DerefOf (Index (Local7, DEF_OFFSET_LINK_HOTPLUG)), varTempLocal1)
+    if (LNotEqual (varTempLocal1, DEF_BASIC_HOTPLUG)) {
       Store ("  No action.[Hotplug type]", Debug)
-      Store ("PciePortEnable Exit", Debug)
-      return (1)
+      Store ("procPciePortTraining Exit", Debug)
+      return (varResultLocal4)
     }
-    // Poweron phy lanes
-    CreateByteField (Local7, DEF_OFFSET_START_PHY_LANE, varStartPhyLane)
-    CreateByteField (Local7, DEF_OFFSET_END_PHY_LANE, varEndPhyLane)
-    procPcieLanePowerControl (varStartPhyLane, varEndPhyLane, 0)	
-    // Enable lanes
-    CreateByteField (Local7, DEF_OFFSET_START_CORE_LANE, varStartCoreLane)
-    CreateByteField (Local7, DEF_OFFSET_END_CORE_LANE, varEndCoreLane)
-    procPcieLaneEnableControl (Arg0, varStartPhyLane, varEndPhyLane, 0)	
-    //Release training
-    procPcieTrainingControl (Arg0, 0)
-    //Train link
-    Store (procPcieCheckDevicePrecence (Arg0), Local1)
-    if (LEqual (Local1, 1)) {
-      Store (" Device detected", Debug)
-      Store (procPcieIsPortReversed (Arg0), varLinkIsLinkReversed)
-      Subtract (procPcieGetLinkWidth (Arg0, 1), procPcieGetLinkWidth (Arg0, 0), Local2)
-      if (LNotEqual (Local2, 0)) {
-         //There is unused lanes after device plugged
-        if (LNotEqual(varLinkIsLinkReversed, 0)) {
-          Add (varStartCoreLane, Local2, Local3)
-          Store (varEndCoreLane, Local4)
-        } else {
-          Subtract (varEndCoreLane, Local2, Local4)
-          Store (varStartCoreLane, Local3)
+    Store (Arg1, varStateLocal2)
+    while (LNotEqual (varStateLocal2, DEF_TRAINING_STATE_EXIT)) {
+      if (LEqual (varStateLocal2, DEF_TRAINING_STATE_RELEASE_TRAINING)) {
+        Store (" State: Release training", Debug)
+        // Remove link speed override
+        Store (0, Index (varOverrideLinkSpeed, Arg0))
+        // Enable link width upconfigure
+        procPciePortIndirectRegisterRMW (Arg0, 0xA2, Not (0x2000), 0x0000)
+        // Request Max link speed for hotplug by going to AC state
+        Store (0, varPsppAcDcOverride)
+        procApplyPsppState ()
+        // Power on/enable port lanes
+        procPcieLaneControl (Arg0, DEF_PCIE_LANE_POWERON)
+        // Release training
+        procPcieTrainingControl (Arg0, 0)
+        // Move to next state to check presence detection
+        Store (DEF_TRAINING_STATE_DETECT_PRESENCE, varStateLocal2)
+        // Initialize retry count
+        Store(0, varCountLocal3)
+      }
+      if (LEqual (varStateLocal2, DEF_TRAINING_STATE_DETECT_PRESENCE)) {
+        Store (" State: Detect presence", Debug)
+        And (procPciePortIndirectRegisterRead (Arg0, 0xa5), 0x3f, varTempLocal1)
+        if (LGreater (varTempLocal1, 0x4)) {
+          // device connection detected move to next state
+          Store (DEF_TRAINING_STATE_PRESENCE_DETECTED, varStateLocal2)
+          // reset retry counter
+          Store(0, varCountLocal3)
+          continue
         }
-        procPcieLaneEnableControl (Arg0, Local3, Local4, 1)
-        if (LGreater (varStartPhyLane, varEndPhyLane)) {
-          Store (varEndPhyLane, Local3)
-          Store (varStartPhyLane, Local4)
+        if (LLess (varCountLocal3, 80)) {
+          Sleep (1)
+          Increment (varCountLocal3)
         } else {
-          Store (varEndPhyLane, Local4)
-          Store (varStartPhyLane, Local3)
+          // detection time expired move to device not present state
+          Store (DEF_TRAINING_STATE_NOT_PRESENT, varStateLocal2)
         }
-        if (LNotEqual(varLinkIsLinkReversed, 0)) {
-          Add (Local3, Local2, Local3)
-        } else {
-          Subtract (Local4, Local2, Local4)
+      }
+      if (LEqual (varStateLocal2, DEF_TRAINING_STATE_PRESENCE_DETECTED)) {
+        Store (" State: Device detected", Debug)
+        Store (procPciePortIndirectRegisterRead (Arg0, 0xa5), varTempLocal1)
+        And (varTempLocal1, 0x3f, varTempLocal1)
+        if (LEqual (varTempLocal1, 0x10)) {
+          Store (DEF_TRAINING_DEVICE_PRESENT, varStateLocal2)
+          continue
+        }
+        if (LLess (varCountLocal3, 80)) {
+          Sleep (1)
+          Increment (varCountLocal3)
+          continue
+        }
+        Store (DEF_TRAINING_STATE_NOT_PRESENT, varStateLocal2)
+
+        if (LEqual (DeRefOf (Index (varOverrideLinkSpeed, Arg0)), DEF_LINK_SPEED_GEN1)) {
+          // GEN2 workaround already applied but device not trained successfully move device not present state
+          continue
+        }
+
+        if (LEqual (procPcieCheckForGen2Workaround (Arg0), TRUE)) {
+          Store (" Request Gen2 workaround", Debug)
+          procPciePortIndirectRegisterRMW (Arg0, 0xA2, Not (0x2000), 0x2000)
+          Store (DEF_LINK_SPEED_GEN1, Index (varOverrideLinkSpeed, Arg0))
+          procPcieSetLinkSpeed (Arg0, DEF_LINK_SPEED_GEN1)
+          Store (DEF_TRAINING_STATE_REQUEST_RESET, varStateLocal2)
         }
-        procPcieLanePowerControl (Local3, Local4, 1)   
       }
-      Store ("PciePortEnable Exit", Debug)
-      return (1)   
-    } 
-    Store (" Device detection fail", Debug)
-    procPciePortDisable (Arg0)  
-    Store ("PciePortEnable Exit", Debug)
-    return (0)
+      if (LEqual (varStateLocal2, DEF_TRAINING_STATE_NOT_PRESENT)) {
+        Store (" State: Device not present", Debug)
+        procPcieTrainingControl (Arg0, 1)
+        procPcieLaneControl (Arg0, DEF_PCIE_LANE_POWEROFF)
+        // Exclude device from PSPP managment since it is not present
+        Store (DEF_LINK_SPEED_GEN1, Index (varOverrideLinkSpeed, Arg0))
+        Store (DEF_TRAINING_STATE_COMPLETE, varStateLocal2)
+      }
+      if (LEqual (varStateLocal2, DEF_TRAINING_STATE_REQUEST_RESET)) {
+        Store (" State: Request Reset", Debug)
+        if (CondRefOf (\_SB.ALIC, Local6)) {
+          Store (" Call ALIC method", Debug)
+          //varTempLocal1  contain port BDF
+          Store(ShiftLeft (Add (Arg0, 2), 3), varTempLocal1)
+          \_SB.ALIC (varTempLocal1, 0)
+          Sleep (2)
+          \_SB.ALIC (varTempLocal1, 1)
+          Store (0, varCountLocal3)
+          Store (DEF_TRAINING_STATE_DETECT_PRESENCE, varStateLocal2)
+          continue
+        }
+        Store (DEF_TRAINING_STATE_NOT_PRESENT, varStateLocal2)
+      }
+      if (LEqual (varStateLocal2, DEF_TRAINING_DEVICE_PRESENT)) {
+        Store (" State: Device present", Debug)
+        Store (DEF_HOTPLUG_STATUS_DEVICE_PRESENT, varResultLocal4)
+        Store (DEF_TRAINING_STATE_COMPLETE, varStateLocal2)
+        procPcieLaneControl (Arg0, DEF_PCIE_LANE_POWEROFFUNUSED)
+      }
+      if (LEqual (varStateLocal2, DEF_TRAINING_STATE_COMPLETE)) {
+
+        Store (1, varPsppAcDcOverride)
+        procApplyPsppState ()
+
+        Store (DEF_TRAINING_STATE_EXIT, varStateLocal2)
+      }
+    }
+    Store ("PciePortTraining Exit", Debug)
+    return (varResultLocal4)
   }
 
-  /*----------------------------------------------------------------------------------------*/
+
+ /*----------------------------------------------------------------------------------------*/
   /**
-   *  Disable PCIe port 
-   *  
-   *  1) Hold training
-   *  2) Disable lanes
-   *  3) Gate lanes
+   *  Lane control
    *
-   *  Arg0 - Port Index
-   *  
+   *  Arg0 	 - Port Index
+   *  Arg1   - 0 - Power off all lanes / 1 - Power on all Lanes / 2 Power off unused lanes
    */
-  Method (procPciePortDisable, 1, NotSerialized) {
-    Store ("PciePortDisable Enter", Debug)   
+
+  Method (procPcieLaneControl, 2, Serialized) {
+    Store ("PcieLaneControl Enter", Debug)
+    Store (Concatenate (" Arg0 : ", ToHexString (Arg0), varStringBuffer), Debug)
+    Store (Concatenate (" Arg1 : ", ToHexString (Arg1), varStringBuffer), Debug)
     Store (procPcieGetPortInfo (Arg0), Local7)
-    CreateByteField (Local7, DEF_OFFSET_LINK_HOTPLUG, varHotplugType)
-    if (LNotEqual (varHotplugType, DEF_BASIC_HOTPLUG)) {
-      Store ("  No action. [Hotplug type]", Debug)
-      Store ("PciePortDisable Exit", Debug)
-      return (0)	
+#ifdef  PCIE_PHY_LANE_POWER_GATE_SUPPORT
+    Store (DerefOf (Index (Local7, DEF_OFFSET_START_PHY_LANE)), varStartPhyLane)
+    Store (DerefOf (Index (Local7, DEF_OFFSET_END_PHY_LANE)), varEndPhyLane)
+#endif
+    Store (DerefOf (Index (Local7, DEF_OFFSET_START_CORE_LANE)), varStartCoreLane)
+    Store (DerefOf (Index (Local7, DEF_OFFSET_END_CORE_LANE)), varEndCoreLane)
+
+    if (LEqual (Arg1, DEF_PCIE_LANE_POWEROFF)) {
+      procPcieLaneEnableControl (Arg0, varStartCoreLane, varEndCoreLane, 1)
+#ifdef  PCIE_PHY_LANE_POWER_GATE_SUPPORT
+      procPcieLanePowerControl (varStartPhyLane, varEndPhyLane, 1)
+#endif
+    }
+    if (LEqual (Arg1, DEF_PCIE_LANE_POWERON)) {
+#ifdef  PCIE_PHY_LANE_POWER_GATE_SUPPORT
+      procPcieLanePowerControl (varStartPhyLane, varEndPhyLane, 0)
+#endif
+      procPcieLaneEnableControl (Arg0, varStartCoreLane, varEndCoreLane, 0)
+    }
+    if (LNotEqual (Arg1, DEF_PCIE_LANE_POWEROFFUNUSED)) {
+      return (0)
+    }
+    Store (procPcieGetLinkWidth (Arg0, DEF_LINKWIDTH_ACTIVE), varActiveLinkWidthLocal2)
+    if (LLessEqual (procPcieGetLinkWidth (Arg0, DEF_LINKWIDTH_MAX_PHY), varActiveLinkWidthLocal2)) {
+      // Active link equal max link width, nothing needs to be done
+      return (0)
+    }
+    Store (procPcieIsPortReversed (Arg0), varIsReversedLocal1)
+    //There is unused lanes after device plugged
+    if (LEqual(varIsReversedLocal1, FALSE)) {
+      Store (" Port Not Reversed", Debug)
+      // Link not reversed
+      Add (varStartCoreLane, varActiveLinkWidthLocal2, Local3)
+      Store (varEndCoreLane, Local4)
+    } else {
+      // Link reversed
+      Store (" Port Reversed", Debug)
+      Subtract (varEndCoreLane, varActiveLinkWidthLocal2, Local4)
+      Store (varStartCoreLane, Local3)
+    }
+    procPcieLaneEnableControl (Arg0, Local3, Local4, 1)
+#ifdef  PCIE_PHY_LANE_POWER_GATE_SUPPORT
+    if (LGreater (varStartPhyLane, varEndPhyLane)) {
+      Store (varEndPhyLane, Local3)
+      Store (varStartPhyLane, Local4)
+    } else {
+      Store (varEndPhyLane, Local4)
+      Store (varStartPhyLane, Local3)
     }
-      //Hold training
-    procPcieTrainingControl (Arg0, 1)
-    CreateByteField (Local7, DEF_OFFSET_START_CORE_LANE, varStartCoreLane)
-    CreateByteField (Local7, DEF_OFFSET_END_CORE_LANE, varEndCoreLane)
-    // Disable lane
-    procPcieLaneEnableControl (Arg0, varStartCoreLane, varEndCoreLane, 1)
-    CreateByteField (Local7, DEF_OFFSET_START_PHY_LANE, varStartPhyLane)
-    CreateByteField (Local7, DEF_OFFSET_END_PHY_LANE, varEndPhyLane)
-    // Poweroff phy lanes
-    procPcieLanePowerControl (varStartPhyLane, varEndPhyLane, 1)
-  
-    Store ("PciePortDisable Exit", Debug)	
+    if (LEqual(varIsReversedLocal1, FALSE)) {
+      // Not reversed
+      Add (Local3, varActiveLinkWidthLocal2, Local3)
+    } else {
+      // Link reversed
+      Subtract (Local4, varActiveLinkWidthLocal2, Local4)
+    }
+    procPcieLanePowerControl (Local3, Local4, 1)
+#endif
     return (0)
   }
+
  /*----------------------------------------------------------------------------------------*/
   /**
+   *  Check if GEN2 workaround applicable
+   *
+   *  Arg0 	 - Port Index
+   *  Retval - TRUE / FALSE
+   */
+
+  Method (procPcieCheckForGen2Workaround, 1, NotSerialized) {
+    Store (Buffer (16) {}, Local1)
+    Store (0x0, Local0)
+    while (LLessEqual (Local0, 0x3)) {
+      Store (procPciePortIndirectRegisterRead (Arg0, Add (Local0, 0xA5)), Local2)
+      Store (Local2,  Index (Local1, Multiply (Local0, 4)))
+      Store (ShiftRight (Local2, 8),   Index (Local1, Add (Multiply (Local0, 4), 1)))
+      Store (ShiftRight (Local2, 16),  Index (Local1, Add (Multiply (Local0, 4), 2)))
+      Store (ShiftRight (Local2, 24),  Index (Local1, Add (Multiply (Local0, 4), 3)))
+      Increment (Local0)
+    }
+    Store (0, Local0)
+    while (LLess (Local0, 15)) {
+      if (LAnd (LEqual (DeRefOf (Index (Local1, Local0)), 0x2a),  LEqual (DeRefOf (Index (Local1, Add (Local0, 1))), 0x9))) {
+        return (TRUE)
+      }
+      Increment (Local0)
+    }
+    return (FALSE)
+  }
+
+  /*----------------------------------------------------------------------------------------*/
+  /**
    *  Is port reversed
-   *  
+   *
    *  Arg0 	 - Port Index
-   *  Retval - 0 - Not reversed / 1 - Reversed
+   *  Retval - 0 - Not reversed / !=0 - Reversed
    */
-  Method (procPcieIsPortReversed , 1, NotSerialized) {
+  Method (procPcieIsPortReversed , 1, Serialized) {
     Store (procPcieGetPortInfo (Arg0), Local7)
-    CreateByteField (Local7, DEF_OFFSET_START_PHY_LANE, varStartPhyLane)
-    CreateByteField (Local7, DEF_OFFSET_END_PHY_LANE, varEndPhyLane)
+
+    Store (DerefOf (Index (Local7, DEF_OFFSET_START_PHY_LANE)), varStartPhyLane)
+    Store (DerefOf (Index (Local7, DEF_OFFSET_END_PHY_LANE)), varEndPhyLane)
     Store (0, Local0)
     if (LGreater (varStartPhyLane, varEndPhyLane)) {
       Store (1, Local0)
     }
     And (procPciePortIndirectRegisterRead (Arg0, 0x50), 0x1, Local1)
-    return (Xor (Local0, Local1))
+    return (And (Xor (Local0, Local1), 0x1))
   }
 
  /*----------------------------------------------------------------------------------------*/
   /**
    *  Training Control
-   *  
+   *
    *  Arg0 	- Port Index
-   *  Arg1  - Hold Training (1) / Release Training (0)  
+   *  Arg1  - Hold Training (1) / Release Training (0)
    */
   Method (procPcieTrainingControl , 2, NotSerialized) {
     Store ("PcieTrainingControl Enter", Debug)
     Store (procPcieGetPortInfo (Arg0), Local7)
-    CreateByteField (Local7, DEF_OFFSET_PORT_ID, varPortId)
-    CreateWordField (Local7, DEF_OFFSET_WRAPPER_ID, varWrapperId)
+    Store (DerefOf (Index (Local7, DEF_OFFSET_PORT_ID)), varPortId)
+    Store (
+      Or (ShiftLeft (DerefOf (Index (Local7, Add (DEF_OFFSET_WRAPPER_ID, 1))), 8), DerefOf (Index (Local7, DEF_OFFSET_WRAPPER_ID))),
+      varWrapperId
+    )
     procIndirectRegisterRMW (0x0, 0xE0, Or (ShiftLeft (varWrapperId, 16), Add (0x800, Multiply (0x100, varPortId))), Not (0x1), Arg1);
     Store ("PcieTrainingControl Exit", Debug)
   }
 
 
-  /*----------------------------------------------------------------------------------------*/
-  /**
-   *  Check device presence
-   *  
-   *  Arg0 	 - Port Index
-   *  Retval - 1 - Device present, 0 - Device not present
-   */
-  Method (procPcieCheckDevicePrecence, 1, NotSerialized) {
-    Store ("PcieCheckDevicePrecence Enter", Debug)
-    Store (0, Local0)
-    Store (0, Local7)
-    while (LLess (Local0, 320)) { // @todo for debug only should be 80
-      And (procPciePortIndirectRegisterRead (Arg0, 0xa5), 0x3f, Local1)
-      if (LEqual (Local1, 0x10)) {
-        Store (1, Local7)
-        Store (320, Local0)
-        Break
-      }
-      Stall (250)
-      Increment (Local0) 
-    }
-    //Store (Concatenate ("Device Presence Status :", ToHexString (Local7)), Debug)
-    Store ("PcieCheckDevicePrecence Exit", Debug)
-    return (Local7) 
-  }
-
- /*----------------------------------------------------------------------------------------*/
+Name (varLinkWidthBuffer, Buffer () {0, 1, 2, 4, 8, 12, 16})
+/*----------------------------------------------------------------------------------------*/
   /**
    *  Get actual negotiated/PHY or core link width
    *
@@ -270,23 +386,29 @@
    *  Retval - Link Width
    */
   Method (procPcieGetLinkWidth, 2, NotSerialized) {
-    if (LEqual (Arg0, 0)){
-       //Get negotiated length 
+    Store ("PcieGetLinkWidth Enter", Debug)
+    Store (Concatenate (" Arg0 : ", ToHexString (Arg0), varStringBuffer), Debug)
+    Store (Concatenate (" Arg1 : ", ToHexString (Arg1), varStringBuffer), Debug)
+
+    if (LEqual (Arg1, DEF_LINKWIDTH_ACTIVE)){
+       //Get negotiated length
       And (ShiftRight (procPciePortIndirectRegisterRead (Arg0, 0xA2), 4), 0x7, Local0)
-      Store (DeRefOf (Index (Buffer (){0, 1, 2, 4, 8, 12, 16}, Local0)), Local1)
+      Store (DeRefOf (Index (varLinkWidthBuffer, Local0)), Local1)
+      Store (Concatenate ("  Active Link Width :", ToHexString (Local1), varStringBuffer), Debug)
     } else {
        //Get phy length
       Store (procPcieGetPortInfo (Arg0), Local7)
-      CreateByteField (Local7, DEF_OFFSET_START_PHY_LANE, varStartPhyLane)
-      CreateByteField (Local7, DEF_OFFSET_END_PHY_LANE, varEndPhyLane)
+      Store (DerefOf (Index (Local7, DEF_OFFSET_START_PHY_LANE)), varStartPhyLane)
+      Store (DerefOf (Index (Local7, DEF_OFFSET_END_PHY_LANE)), varEndPhyLane)
       if (LGreater (varStartPhyLane, varEndPhyLane)) {
         Subtract (varStartPhyLane, varEndPhyLane, Local1)
       } else {
         Subtract (varEndPhyLane, varStartPhyLane, Local1)
       }
-      Increment (Local1)         
+      Increment (Local1)
+      Store (Concatenate ("  PHY Link Width :", ToHexString (Local1), varStringBuffer), Debug)
     }
-    //Store (Concatenate ("Link Width :", ToHexString (Local7)), Debug)
+    Store ("PcieGetLinkWidth Exit", Debug)
     return (Local1)
   }
 
@@ -297,16 +419,21 @@
    *  Arg0 - Port Index
    *  Arg1 - Start Lane
    *  Arg2 - End Lane
-   *  Arg3 - Enable(0) / Disable(1) 
+   *  Arg3 - Enable(0) / Disable(1)
    */
-  Method (procPcieLaneEnableControl, 4, NotSerialized) {
+  Method (procPcieLaneEnableControl, 4, Serialized) {
     Store ("PcieLaneEnableControl Enter", Debug)
-    Name (varStartCoreLane, 0)
-    Name (varEndCoreLane, 0)
+    Store (Concatenate (" Arg0 : ", ToHexString (Arg0), varStringBuffer), Debug)
+    Store (Concatenate (" Arg1 : ", ToHexString (Arg1), varStringBuffer), Debug)
+    Store (Concatenate (" Arg2 : ", ToHexString (Arg2), varStringBuffer), Debug)
+    Store (Concatenate (" Arg3 : ", ToHexString (Arg3), varStringBuffer), Debug)
     Store (procPcieGetPortInfo (Arg0), Local7)
     Store (Arg1, varStartCoreLane)
     Store (Arg2, varEndCoreLane)
-    CreateWordField (Local7, DEF_OFFSET_WRAPPER_ID, varWrapperId)
+    Store (
+      Or (ShiftLeft (DerefOf (Index (Local7, Add (DEF_OFFSET_WRAPPER_ID, 1))), 8), DerefOf (Index (Local7, DEF_OFFSET_WRAPPER_ID))),
+      varWrapperId
+    )
     if (LGreater (varStartCoreLane, varEndCoreLane)) {
       Subtract (varStartCoreLane, varEndCoreLane, Local1)
       Store (varEndCoreLane, Local2)
@@ -314,13 +441,13 @@
       Subtract (varEndCoreLane, varStartCoreLane, Local1)
       Store (varStartCoreLane, Local2)
     }
-    ShiftLeft (Subtract (ShiftLeft (1, Add (Local1, 1)), 1), Local2, Local1)
-    //Store (Concatenate ("Lane Bitmap :", ToHexString (Local1)), Debug)
-    if (Lequal (Arg3, 0)) {
-      procIndirectRegisterRMW (0x0, 0xE0, Or (ShiftLeft (varWrapperId, 16), 0x8023), 0xffffffff, Local1);
-    } else {
-      procIndirectRegisterRMW (0x0, 0xE0, Or (ShiftLeft (varWrapperId, 16), 0x8023), Not (Local1), 0x0);
+    ShiftLeft (Subtract (ShiftLeft (1, Add (Local1, 1)), 1), Local2, varLaneBitmapOrMaskLocal3)
+    Store (Not (varLaneBitmapOrMaskLocal3), varLaneBitmapAndMaskLocal4)
+    Store (Concatenate ("Lane Bitmap : ", ToHexString (varLaneBitmapOrMaskLocal3), varStringBuffer), Debug)
+    if (Lequal (Arg3, 1)) {
+      Store (0, varLaneBitmapOrMaskLocal3)
     } 
+    procIndirectRegisterRMW (0x0, 0xE0, Or (ShiftLeft (varWrapperId, 16), 0x8023), varLaneBitmapAndMaskLocal4, varLaneBitmapOrMaskLocal3);
     Stall (10)
     Store ("PcieLaneEnableControl Exit", Debug)
   }
diff --git a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibPspp.esl b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibPspp.esl
index 70d6a93b84..2bfabc9077 100644
--- a/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibPspp.esl
+++ b/src/vendorcode/amd/agesa/f14/Proc/GNB/Modules/GnbPcieAlibV1/PcieAlibPspp.esl
@@ -12,36 +12,36 @@
 *
 */
 /*
- *****************************************************************************
- *
- * Copyright (c) 2011, Advanced Micro Devices, Inc.
- * All rights reserved.
- * 
- * 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 Advanced Micro Devices, Inc. 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 ADVANCED MICRO DEVICES, INC. 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.
- * 
- * ***************************************************************************
- *
- */
+*****************************************************************************
+*
+* Copyright (c) 2011, Advanced Micro Devices, Inc.
+* All rights reserved.
+*
+* 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 Advanced Micro Devices, Inc. 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 ADVANCED MICRO DEVICES, INC. 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.
+*
+****************************************************************************
+*
+*/
 
   /*----------------------------------------------------------------------------------------*/
   /**
@@ -112,26 +112,23 @@
 
   /*----------------------------------------------------------------------------------------*/
   /**
-   *  Max Port GEN capability
+   *  Max Port link speed
    *
    */
-  Name (
-    AD06,
-    Package () {
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00
-    }
-  )
+  Name (AD06, Package () {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})
+
+  Alias (AD06, varMaxLinkSpeed)
 
-  Alias (
-    AD06,
-    varPsppMaxPortCapabilityArray
-  )
+
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Max link speed that was changed during runtime (hotplug for instance)
+   *
+   */
+
+  Name (AD08, Package () {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})
+
+  Alias (AD08,  varOverrideLinkSpeed)
 
   /*----------------------------------------------------------------------------------------*/
   /**
@@ -141,10 +138,7 @@
    *                         1 (Started)
    */
 
-  Name (
-    varPsppPolicyService,
-    0x0
-  )
+  Name (varPsppPolicyService, 0x0 )
 
   /*----------------------------------------------------------------------------------------*/
   /**
@@ -154,85 +148,100 @@
    *                     1 (DC)
    */
 
-  Name (
-    varPsppAcDcState,
-    0x0
-  )
+  Name (varPsppAcDcState, 0x0)
+  Name (varPsppAcDcOverride, 0x1)
 
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Client ID array
+   *
+   */
 
-  Name (
-    varPsppClientIdArray,
-    Package () {
-      0x0000,
-      0x0000,
-      0x0000,
-      0x0000,
-      0x0000,
-      0x0000,
-      0x0000
-    }
-  )
+  Name (varPsppClientIdArray,
+    Package () {0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000}
+    )
 
-  Name (
-    varPsppClientCapabilityArray,
-    Package () {
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00
-    }
-  )
+  Name (varDefaultPsppClientIdArray,
+    Package () {0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000}
+    )
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  LInk speed requested by device driver
+   *
+   */
 
+  Name (varRequestedLinkSpeed, Package () {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})
+
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Current link speed
+   *
+   */
+  Name (AD09, Package () {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 })
+  Alias (AD09,  varCurrentLinkSpeed)
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Template link speed
+   *
+   */
   Name (
-    varPsppCurrentCapabilityArray,
-    Package () {
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00,
-      0x00
-    }
-  )
-  Name (
-    varDefaultGen1CapabilityArray,
+    varGen1LinkSpeedTemplate,
     Package () {
-      0x2,
-      0x2,
-      0x2,
-      0x2,
-      0x2,
-      0x2,
-      0x2
-      }
-    )
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1,
+    DEF_LINK_SPEED_GEN1
+    })
 
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Template link speed
+   *
+   */
+  Name (varLowVoltageRequest, Package () {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 })
 
   /*----------------------------------------------------------------------------------------*/
   /**
+   *  Global varuable
+   *
+   */
+  Name (varPortIndex, 0)
+  /*----------------------------------------------------------------------------------------*/
+  /**
    *  Report AC/DC state
    *
    *  Arg0 - Data Buffer
    */
   Method (procPsppReportAcDsState, 1, Serialized) {
     Store ("PsppReportAcDsState Enter", Debug)
-    CreateByteField (Arg0, 0x2, varArgAcDcState)
 
-    Store ("AC/DC state = ", Debug)
-    Store (varArgAcDcState, Debug)
-    if (LEqual (varArgAcDcState, varPsppAcDcState)) {
+    Store (DeRefOf (Index (Arg0, 0x2)), varArgAcDcStateLocal1)
+    Store (Concatenate ("  AC/DC state: ", ToHexString (varArgAcDcStateLocal1), varStringBuffer), Debug)
+
+    Store (procPsppGetAcDcState(), varCurrentAcDcStateLocal0)
+    Store (varArgAcDcStateLocal1, varPsppAcDcState)
+
+    Or (ShiftLeft (1, DEF_SCARTCH_PSPP_ACDC_OFFSET), ShiftLeft (1, DEF_SCARTCH_PSPP_ACDC_OVR_OFFSET), Local2)
+    Or (ShiftLeft (varPsppAcDcState, DEF_SCARTCH_PSPP_ACDC_OFFSET), ShiftLeft (varPsppAcDcOverride, DEF_SCARTCH_PSPP_ACDC_OVR_OFFSET), Local3)
+    procIndirectRegisterRMW (0x0, 0x60, 0xF4, Not (Local2), And (Local2, Local3))
+
+
+    if (LEqual (varArgAcDcStateLocal1, varCurrentAcDcStateLocal0)) {
       Store ("  No action. [AC/DC state not changed]", Debug)
       Store ("PsppReportAcDsState Exit", Debug)
       return (0)
     }
-    Store (varArgAcDcState, varPsppAcDcState)
+
+    // Disable both APM (boost) and PDM flow on DC event enable it on AC.
+    procApmPdmActivate(varPsppAcDcState)
+
     // Set DPM state for Power Saving, due to this policy will not attend ApplyPsppState service.
     if (LEqual (varPsppPolicy, DEF_PSPP_POLICY_POWERSAVING)) {
-      procNbLclkDpmActivate(1, varPsppAcDcState)
+      procNbLclkDpmActivate(1, procPsppGetAcDcState())
     }
     if (LOr (LLessEqual (varPsppPolicy, DEF_PSPP_POLICY_PERFORMANCE), LGreaterEqual (varPsppPolicy, DEF_PSPP_POLICY_POWERSAVING))) {
       Store ("  No action. [Policy type]", Debug)
@@ -255,15 +264,31 @@
    *
    *  Arg0 - Data Buffer
    */
-  Method (procPsppPerformanceRequest, 1) {
-    Store ("PsppPerformanceRequest Enter", Debug)
+  Method (procPsppPerformanceRequest, 1, NotSerialized) {
+    Store (procPsppProcessPerformanceRequest (Arg0), Local7)
+    Store (DeRefOf (Index (Local7, 2)), varReturnStatusLocal0)
+    if (LNotEqual (varReturnStatusLocal0, 2)) {
+      return (Local7)
+    }
+    procApplyPsppState ()
+    return (Local7)
+  }
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  PCIe Performance Request
+   *
+   *  Arg0 - Data Buffer
+   */
+  Method (procPsppProcessPerformanceRequest, 1, NotSerialized) {
+    Store ("PsppProcessPerformanceRequest Enter", Debug)
     Name (varClientBus, 0)
-    Name (varPortIndex, 0)
-    Store (Buffer (256) {}, Local7)
+    Store (0, varPortIndex)
+    Store (Buffer (10) {}, Local7)
     CreateWordField (Local7, 0x0, varReturnBufferLength)
     Store (3, varReturnBufferLength)
     CreateByteField (Local7, 0x2, varReturnStatus)
     Store (1, varReturnStatus)
+
     if (LOr (LLessEqual (varPsppPolicy, DEF_PSPP_POLICY_PERFORMANCE), LGreaterEqual (varPsppPolicy, DEF_PSPP_POLICY_POWERSAVING))) {
       Store ("  No action. [Policy type]", Debug)
       Store ("PsppPerformanceRequest Exit", Debug)
@@ -280,56 +305,58 @@
     CreateByteField (Arg0, 0x8, varRequestType)
     CreateByteField (Arg0, 0x9, varRequestData)
 
-    Store ("  Client ID:", Debug)
-    Store (varClientId, Debug)
-    Store ("  Valid Flags:", Debug)
-    Store (varValidFlag, Debug)
-    Store ("  Flags:", Debug)
-    Store (varFlag, Debug)
-    Store ("  Request Type:", Debug)
-    Store (varRequestType, Debug)
-    Store ("  Request Data:", Debug)
-    Store (varRequestData, Debug)
+    Store (Concatenate ("  Client ID   : ", ToHexString (varClientId), varStringBuffer), Debug)
+    Store (Concatenate ("  Valid Flags : ", ToHexString (varValidFlag), varStringBuffer), Debug)
+    Store (Concatenate ("  Flags       : ", ToHexString (varFlag), varStringBuffer), Debug)
+    Store (Concatenate ("  Request Type: ", ToHexString (varRequestType), varStringBuffer), Debug)
+    Store (Concatenate ("  Request Data: ", ToHexString (varRequestData), varStringBuffer), Debug)
+
 
     And (ShiftRight (varClientId, 8), 0xff, varClientBus)
-    While (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
-      if (LEqual (DeRefOf (Index (varPsppMaxPortCapabilityArray, varPortIndex)), 0)) {
-        Increment (varPortIndex)
-        Continue
-      }
-      Store (procPciDwordRead (ShiftLeft (Add( varPortIndex, 2), 3), 0x18), Local1)
-      And (ShiftRight (Local1, 16), 0xff, Local2)             //Local2 Port Subordinate Bus number
-      And (ShiftRight (Local1, 8), 0xff, Local1)              //Local1 Port Secondary Bus number
-      if (LAnd (LLess (varClientBus, Local1), LGreater (varClientBus, Local2))) {
-        Increment (varPortIndex)
-        Continue
-      }
-      Store ("Performance request for port index", Debug)
-      Store (varPortIndex, Debug)
-
-      if (LEqual (DeRefOf (Index (varPsppClientIdArray, varPortIndex)), 0x0000)) {
-        Store (varClientId, Index (varPsppClientIdArray, varPortIndex))
-      } ElseIf (LNotEqual (DeRefOf (Index (varPsppClientIdArray, varPortIndex)), varClientId)) {
-        // We already have registered client
-        Store ("  No action. [Unsupported request]", Debug)
-        Store ("PsppPerformanceRequest Exit", Debug)
-        return (Local7)
-      }
-      if (LEqual (varRequestData, 0)) {
-        Store (0x0000, Index (varPsppClientIdArray, varPortIndex))
-      } else {
-        if (LEqual (And (varValidFlag, varFlag), 0x1)) {
-          Store (DerefOf (Index (varPsppMaxPortCapabilityArray, varPortIndex)), Index (varPsppClientCapabilityArray, varPortIndex))
-        } else {
-          Store (varRequestData, Index (varPsppClientCapabilityArray, varPortIndex))
+    while (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
+      if (LEqual (procChecPortAllocated (varPortIndex), DEF_PORT_ALLOCATED)) {
+        Store (procPciDwordRead (ShiftLeft (Add( varPortIndex, 2), 3), 0x18), Local1)
+        And (ShiftRight (Local1, 16), 0xff, varSubordinateBusLocal2)             //Local2 Port Subordinate Bus number
+        And (ShiftRight (Local1, 8), 0xff, varSecondaryBusLocal1)                //Local1 Port Secondary Bus number
+        if (LAnd (LGreaterEqual (varClientBus, Local1), LLessEqual(varClientBus, Local2))) {
+          break
         }
       }
-      procApplyPsppState ()
-      Store (2, varReturnStatus)
+      Increment (varPortIndex)
+    }
+    if (LGreater (varPortIndex, varMaxPortIndexNumber)) {
+      Store ("PsppPerformanceRequest Exit", Debug)
+      return (Local7)
+    }
+
+    Store (Concatenate ("  Performance request for port index : ", ToHexString (varPortIndex), Local6), Debug)
+
+    if (LEqual (DeRefOf (Index (varPsppClientIdArray, varPortIndex)), 0x0000)) {
+      Store (varClientId, Index (varPsppClientIdArray, varPortIndex))
+    } ElseIf (LNotEqual (DeRefOf (Index (varPsppClientIdArray, varPortIndex)), varClientId)) {
+      // We already have registered client
+      Store ("  No action. [Unsupported request]", Debug)
       Store ("PsppPerformanceRequest Exit", Debug)
       return (Local7)
     }
-    Store ("PsppPerformanceRequest Exit", Debug)
+    Store (0, Index (varLowVoltageRequest, varPortIndex))
+    if (LEqual (varRequestData, 0)) {
+      Store (0x0000, Index (varPsppClientIdArray, varPortIndex))
+    }
+    if (LEqual (varRequestData, 1)) {
+      Store (1, Index (varLowVoltageRequest, varPortIndex))
+    }
+    if (LEqual (varRequestData, 2)) {
+      Store (DEF_LINK_SPEED_GEN1, Index (varRequestedLinkSpeed, varPortIndex))
+    }
+    if (LEqual (varRequestData, 3)) {
+      Store (DEF_LINK_SPEED_GEN2, Index (varRequestedLinkSpeed, varPortIndex))
+    }
+    if (LEqual (And (varValidFlag, varFlag), 0x1)) {
+      Store (DerefOf (Index (varMaxLinkSpeed, varPortIndex)), Index (varRequestedLinkSpeed, varPortIndex))
+    }
+    Store (2, varReturnStatus)
+    Store ("PsppProcessPerformanceRequest Exit", Debug)
     return (Local7)
   }
 
@@ -339,39 +366,84 @@
    *
    *  Arg0 - Data Buffer
    */
+
+  Method (procChecPortAllocated, 1, Serialized) {
+    if (LEqual (DeRefOf (Index (varMaxLinkSpeed, Arg0)), 0)) {
+      return (DEF_PORT_NOT_ALLOCATED)
+    }
+    return (DEF_PORT_ALLOCATED)
+  }
+
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  PSPP Start/Stop Management Request
+   *
+   *  Arg0 - Data Buffer
+   */
   Method (procPsppControl, 1, Serialized) {
     Store ("PsppControl Enter", Debug)
     Store (Buffer (256) {}, Local7)
-    CreateWordField (Local7, 0x0, varReturnBufferLength)
-    Store (3, varReturnBufferLength)
-    CreateByteField (Local7, 0x2, varReturnStatus)
-    CreateByteField (Arg0, 0x2, varArgPsppRequest)
-    Store (varArgPsppRequest, varPsppPolicyService)
+    Store (3, Index (Local7, 0x0)) // Return Buffer Length
+    Store (0, Index (Local7, 0x1)) // Return Buffer Length
+    Store (0, Index (Local7, 0x2)) // Return Status
+
+    Store (DerefOf (Index (Arg0, 0x2)), varPsppPolicyService)
+
+    Store (procIndirectRegisterRead (0x0, 0x60, 0xF4), varPsppScratchLocal0)
+
+    if (LEqual (varPsppPolicyService, DEF_PSPP_POLICY_START)) {
+      if (LEqual (And (varPsppScratchLocal0, 1), DEF_PSPP_POLICY_START)) {
+      // Policy already started
+        Store (" No action. [Policy already started]", Debug)
+        Store ("PsppControl Exit", Debug)
+        return (Local7)
+      }
+      Or (varPsppScratchLocal0, DEF_PSPP_POLICY_START, varPsppScratchLocal0)
+    }
+    if (LEqual (varPsppPolicyService, DEF_PSPP_POLICY_STOP)) {
+      if (LEqual (And (varPsppScratchLocal0, 1), DEF_PSPP_POLICY_STOP)) {
+        // Policy already stopped
+        Store ("  No action. [Policy already stopped]", Debug)
+        Store ("PsppControl Exit", Debug)
+        return (Local7)
+      }
+      And (varPsppScratchLocal0, Not (DEF_PSPP_POLICY_START), varPsppScratchLocal0)
+    }
+    Or (varPsppScratchLocal0, Shiftleft (varPsppPolicy, DEF_SCARTCH_PSPP_POLICY_OFFSET), varPsppScratchLocal0)
+    procIndirectRegisterWrite (0x0, 0x60, 0xF4, varPsppScratchLocal0)
+
+    procCopyPackage (RefOf (varDefaultPsppClientIdArray), RefOf (varPsppClientIdArray))
+
+    // Reevaluate APM/PDM state here on S3 resume while staying on DC.
+    procApmPdmActivate(varPsppAcDcState)
+
     // Set DPM state for PSPP Power Saving, due to this policy will not attend ApplyPsppState service.
     if (LEqual (varPsppPolicy, DEF_PSPP_POLICY_POWERSAVING)) {
-      procNbLclkDpmActivate(1, varPsppAcDcState)
+      procNbLclkDpmActivate(DEF_LINK_SPEED_GEN1, procPsppGetAcDcState())
     }
     //Reevaluate PCIe speed for all devices base on PSPP state switch to boot up voltage
     if (LAnd (LGreater (varPsppPolicy, DEF_PSPP_POLICY_PERFORMANCE), LLess (varPsppPolicy, DEF_PSPP_POLICY_POWERSAVING))) {
       // Load default speed capability state
       if (LEqual (varPsppPolicy, DEF_PSPP_POLICY_BALANCEHIGH)) {
-        procCopyPackage (RefOf (varPsppMaxPortCapabilityArray), RefOf (varPsppCurrentCapabilityArray))
+        procCopyPackage (RefOf (varMaxLinkSpeed), RefOf (varCurrentLinkSpeed))
+        Store (0, varPortIndex)
+        while (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
+          if (LNotEqual (DeRefOf (Index (varOverrideLinkSpeed, varPortIndex)), 0)) {
+            Store (DeRefOf (Index (varOverrideLinkSpeed, varPortIndex)), Index (varCurrentLinkSpeed, varPortIndex))
+          }
+          Increment (varPortIndex)
+        }
       } else {
-        procCopyPackage (RefOf (varDefaultGen1CapabilityArray), RefOf (varPsppCurrentCapabilityArray))
-      }
-      // Unregister all clients
-      if (LEqual (varPsppPolicyService, DEF_PSPP_POLICY_STOP)) {
-        Name (varDefaultPsppClientIdArray, Package () {0x0, 0x0, 0x0, 0x0, 0x0, 0x0})
-        procCopyPackage (RefOf (varDefaultPsppClientIdArray), RefOf (varPsppClientIdArray))
+        procCopyPackage (RefOf (varGen1LinkSpeedTemplate), RefOf (varCurrentLinkSpeed))
       }
       procApplyPsppState ()
     }
-    Store (3, varReturnBufferLength)
-    Store (0, varReturnStatus)
     Store ("PsppControl Exit", Debug)
     return (Local7)
   }
 
+  Name (varNewLinkSpeed, Package () {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})
+
   /*----------------------------------------------------------------------------------------*/
   /**
    *  Evaluate PCIe speed on all links according to PSPP state and client requests
@@ -381,47 +453,46 @@
    */
   Method (procApplyPsppState, 0, Serialized) {
     Store ("ApplyPsppState Enter", Debug)
-    Name (varPortIndex, 0)
-    Name (varLowPowerMode, 0)
-    Name (varPcieCapabilityArray, Package () {0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02})
-
     Store (0, varPortIndex)
-    While (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
-      if (LNotEqual (DeRefOf (Index (varPsppMaxPortCapabilityArray, varPortIndex)), 0)) {
-        Store (procGetPortRequestedCapability (varPortIndex), Index (varPcieCapabilityArray, varPortIndex))
+
+    procCopyPackage (RefOf (varGen1LinkSpeedTemplate), RefOf (varNewLinkSpeed))
+    while (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
+      if (LEqual (procChecPortAllocated(varPortIndex), DEF_PORT_ALLOCATED)) {
+        Store (procGetPortRequestedCapability (varPortIndex), Index (varNewLinkSpeed, varPortIndex))
       }
       Increment (varPortIndex)
     }
-    if (LNotEqual(Match (varPcieCapabilityArray, MEQ, 0x01, MTR, 0, 0), ONES)) {
-      procCopyPackage (RefOf (varDefaultGen1CapabilityArray), RefOf (varPcieCapabilityArray))
+    if (LNotEqual(Match (varLowVoltageRequest, MEQ, 0x01, MTR, 0, 0), ONES)) {
+      procCopyPackage (RefOf (varGen1LinkSpeedTemplate), RefOf (varNewLinkSpeed))
     }
-    if (LNotEqual(Match (varPcieCapabilityArray, MEQ, 0x03, MTR, 0, 0), ONES)) {
+    if (LNotEqual(Match (varNewLinkSpeed, MEQ, DEF_LINK_SPEED_GEN2, MTR, 0, 0), ONES)) {
       // Set GEN2 voltage
       Store ("Set GEN2 VID", Debug)
       procPcieSetVoltage (varGen2Vid, 1)
-      procPcieAdjustPll (2)
-      procNbLclkDpmActivate(2, varPsppAcDcState)
+      procPcieAdjustPll (DEF_LINK_SPEED_GEN2)
+      procNbLclkDpmActivate(DEF_LINK_SPEED_GEN2, procPsppGetAcDcState())
     }
     Store (0, varPortIndex)
-    While (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
-      if (LEqual (DeRefOf (Index (varPsppMaxPortCapabilityArray, varPortIndex)), 0)) {
+    while (LLessEqual (varPortIndex, varMaxPortIndexNumber)) {
+      if (LEqual (procChecPortAllocated(varPortIndex), DEF_PORT_NOT_ALLOCATED)) {
         Increment (varPortIndex)
-        Continue
+        continue
       }
-      Store (procGetPortCurrentCapability (varPortIndex), Local0)
-      Store (DerefOf (Index (varPcieCapabilityArray, varPortIndex)), Local2)
-      if (LEqual (Local0, Local2)) {
+      Store (DerefOf (Index (varCurrentLinkSpeed, varPortIndex)), varCurrentLinkSpeedLocal0)
+      Store (DerefOf (Index (varNewLinkSpeed, varPortIndex)), varNewLinkSpeedLocal2)
+      if (LEqual (varCurrentLinkSpeedLocal0, varNewLinkSpeedLocal2)) {
         Increment (varPortIndex)
-        Continue
+        continue
       }
-      procSetPortCapabilityAndSpeed (varPortIndex, Local2, 0)
+      Store (varNewLinkSpeedLocal2, Index (varCurrentLinkSpeed, varPortIndex))
+      procSetPortCapabilityAndSpeed (varPortIndex, varNewLinkSpeedLocal2)
       Increment (varPortIndex)
     }
-    if (LEqual(Match (varPcieCapabilityArray, MEQ, 0x03, MTR, 0, 0), ONES)) {
+    if (LEqual(Match (varNewLinkSpeed, MEQ, DEF_LINK_SPEED_GEN2, MTR, 0, 0), ONES)) {
       // Set GEN1 voltage
       Store ("Set GEN1 VID", Debug)
-      procNbLclkDpmActivate(1, varPsppAcDcState)
-      procPcieAdjustPll (1)
+      procNbLclkDpmActivate(DEF_LINK_SPEED_GEN1, procPsppGetAcDcState())
+      procPcieAdjustPll (DEF_LINK_SPEED_GEN1)
       procPcieSetVoltage (varGen1Vid, 0)
     }
     Store ("ApplyPsppState Exit", Debug)
@@ -434,200 +505,218 @@
    *
    */
   Method (procGetPortRequestedCapability, 1) {
-    Store (0x3, Local0)
+    Store (DEF_LINK_SPEED_GEN2, Local0)
     if (LEqual (DerefOf (Index (varPsppClientIdArray, Arg0)), 0x0000)) {
-      if (LOr (LEqual (varPsppAcDcState, DEF_PSPP_STATE_DC), LEqual (varPsppPolicy, DEF_PSPP_POLICY_BALANCELOW))) {
+      if (LOr (LEqual (procPsppGetAcDcState(), DEF_PSPP_STATE_DC), LEqual (varPsppPolicy, DEF_PSPP_POLICY_BALANCELOW))) {
         // Default policy cap to GEN1
-        Store (0x2, Local0)
+        Store (DEF_LINK_SPEED_GEN1, Local0)
+      }
+      if (LNotEqual (DerefOf (Index (varOverrideLinkSpeed, Arg0)), 0)) {
+        Store (DerefOf (Index (varOverrideLinkSpeed, Arg0)), Local0)
       }
     } else {
-      Store (DerefOf (Index (varPsppClientCapabilityArray, Arg0)), Local0)
+      Store (DerefOf (Index (varRequestedLinkSpeed, Arg0)), Local0)
     }
     return (Local0)
   }
 
   /*----------------------------------------------------------------------------------------*/
   /**
-   *  Read PCI config register
+   *  Set capability and speed
    *
    *  Arg0 - Port Index
-   *
+   *  Arg1 - Link speed
    */
-  Method (procGetPortCurrentCapability, 1) {
-    return (DerefOf (Index (varPsppCurrentCapabilityArray, Arg0)))
+  Method (procSetPortCapabilityAndSpeed, 2, NotSerialized) {
+    Store ("SetPortCapabilityAndSpeed Enter", Debug)
+    Store (Concatenate ("  Port Index : ", ToHexString (Arg0), varStringBuffer), Debug)
+    Store (Concatenate ("  Speed      : ", ToHexString (Arg1), varStringBuffer), Debug)
+
+    //UnHide UMI port
+    if (LEqual (Arg0, 6)) {
+      procIndirectRegisterRMW (0x0, 0x60, 0x80, Not (0x40), 0x40);
+    }
+
+    procPcieSetLinkSpeed (Arg0, Arg1)
+
+    // Programming for LcInitSpdChgWithCsrEn
+    if (LNotEqual (DeRefOf (Index (varPsppClientIdArray, Arg0)), 0x0000)) {
+      // Registered port, LcInitSpdChgWithCsrEn = 0.
+      procPciePortIndirectRegisterRMW (Arg0, 0xA1, Not (0x00001000), 0x0)
+    } else {
+      procPciePortIndirectRegisterRMW (Arg0, 0xA1, Not (0x00001000), 0x00001000)
+    }
+
+    // Determine port PCI address and check port present
+    Store (ShiftLeft (Add( Arg0, 2), 3), varPortBdfLocal1)
+    And (procPciDwordRead (varPortBdfLocal1, 0x70), 0x400000, varPortPresentLocal3)
+    if (LNotEqual (varPortPresentLocal3, 0)) {
+      procDisableAndSaveAspm (Arg0)
+      Store (1, Local2)
+      while (Local2) {
+        //retrain port
+        procPciDwordRMW (varPortBdfLocal1, 0x68, Not (0x00000000), 0x20)
+        Sleep (30)
+        while (And (procPciDwordRead (varPortBdfLocal1, 0x68), 0x08000000)) {
+          Sleep (10)
+        }
+        Store (0, Local2)
+        if (LEqual (Arg1, DEF_LINK_SPEED_GEN1)) {
+          Store (procPciePortIndirectRegisterRead (Arg0, 0xA4), varLcCurrentDataRateLocal4)
+          if (LNotEqual (And (varLcCurrentDataRateLocal4, 0x800), 0)) {
+            Store (1, Local2)
+          }
+        }
+      }
+      procRestoreAspm (Arg0)
+    } else {
+      Store ("  Device not present. Set capability and speed only", Debug)
+    }
+    //Hide UMI port
+    if (LEqual (Arg0, 6)) {
+      procIndirectRegisterRMW (0x0, 0x60, 0x80, Not (0x40), 0x00);
+    }
+    Store ("SetPortCapabilityAndSpeed Exit", Debug)
   }
 
+  Name (varPcieLinkControlArray, Package () {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0})
+  Name (varPcieLinkControlOffset, 0)
+  Name (varPcieLinkControlData, 0)
+
   /*----------------------------------------------------------------------------------------*/
   /**
-   *  Set capability and speed
+   *   Disable and save ASPM state
    *
    *  Arg0 - Port Index
-   *  Arg1 - Capability
-   *  Arg2 - Speed
    */
-  Method (procSetPortCapabilityAndSpeed, 3) {
-    Store ("SetPortCapabilityAndSpeed Enter", Debug)
-    if (LOr (LEqual (Arg1, 0x2), LEqual (Arg1, 0x3))) {
-      Store ("Port Index = ", Debug)
-      Store (Arg0, Debug)
-      Store ("Cap   = ", Debug)
-      Store (Arg1, Debug)
-      Store ("Speed = ", Debug)
-      Store (Arg2, Debug)
-
-      Name (varDxF0xE4_xA4, 0x20000001)
-      Name (varPortPresent, 0x00000000)
-      Name (varDxF0x88, 0x00000002)
-      Name (varAXCFGx68_PmCtrl, 0x00000000)
-      Name (varLcCurrentDataRate,0x00000000)
-      Name (varSecondaryBus, 0x00000000)
-      Name (varHeaderType, 0x00000000)
-      Name (varMultiFunction, 0x00000000)
-      Name (varPcieLinkControlOffset, 0x00000000)
-      Name (varPcieLinkControlData, 0x00000000)
-      Name (varPcieLinkControlArray, Package () {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0})
-
-      
-      //If request for UMI unhihe port congig space
-      if (LEqual (Arg0, 6)) {
-        procIndirectRegisterRMW (0x0, 0x60, 0x80, Not (0x40), 0x40);
+  Method (procDisableAndSaveAspm, 1, Serialized) {
+    Store (0, varPcieLinkControlOffset)
+    Store (0, varPcieLinkControlData)
+
+    Store (ShiftLeft (Add( Arg0, 2), 3), varPortBdfLocal1)
+    if (LEqual (Arg0, 6)) {
+      Store ("  Disable SB ASPM", Debug)
+      Store (procPcieSbAspmControl (0, 0),  Index (varPcieLinkControlArray, 0))
+      Store (Concatenate ("  PcieLinkControl Data : ", ToHexString (DerefOf(Index (varPcieLinkControlArray, 0))), varStringBuffer), Debug)
+      procPcieSbAspmControl (0, 1)
+      return (0)
+    }
 
-      }
-      Store (Arg1, Index (varPsppCurrentCapabilityArray, Arg0))
-      if (LEqual (Arg1, 0x2)) {
-        //Gen1
-        Store (0x00000000, varDxF0xE4_xA4)
-        Store (0x21, varDxF0x88)
-      }
+    Store (procPciDwordRead (varPortBdfLocal1, 0x18), varTempLocal3)
+    Store (And (ShiftRight (varTempLocal3, 8), 0xFF),  varTempLocal3)
 
-      // Programming for LcInitSpdChgWithCsrEn
-      if (LNotEqual (DeRefOf (Index (varPsppClientIdArray, Arg0)), 0x0000)) {
-      	// Registered port, LcInitSpdChgWithCsrEn = 0.
-      	procPciePortIndirectRegisterRMW (Arg0, 0xA1, Not (0x00001000), 0x0)
-      } else {
-        procPciePortIndirectRegisterRMW (Arg0, 0xA1, Not (0x00001000), 0x00001000)
+    Store (Concatenate ("  Disable EP ASPM on Secondary Bus : ", ToHexString (varTempLocal3), varStringBuffer), Debug)
+
+    Store (ShiftLeft (varTempLocal3, 8), varEndpointBdfLocal2)
+    Store (procPciDwordRead (varEndpointBdfLocal2, 0xC), varTempLocal3)
+    Store (And (ShiftRight (varTempLocal3, 16), 0xFF),  varTempLocal3)
+
+    Store (Concatenate ("  EP Header type : ", ToHexString (varTempLocal3), varStringBuffer), Debug)
+
+    if (LNotEqual (And (varTempLocal3, 0x80), 0)) {
+      Store (0x7, varMaxFunctionLocal0)
+    } else {
+      Store (0x0, varMaxFunctionLocal0)
+    }
+    Store (0, varFunctionLocal4)
+    while (LLessEqual (varFunctionLocal4, varMaxFunctionLocal0)) {
+      //Find PcieLinkControl register offset = PcieCapPtr + 0x10
+      Store (procFindPciCapability (Add (varEndpointBdfLocal2, varFunctionLocal4), 0x10), varPcieLinkControlOffset)
+      if (LEqual (varPcieLinkControlOffset, 0)) {
+        Increment (varFunctionLocal4)
+        continue
       }
+      Add (varPcieLinkControlOffset, 0x10, varPcieLinkControlOffset)
 
-      // Initialize port
-      procPciePortIndirectRegisterRMW (Arg0, 0xA4, Not (0x20000001), varDxF0xE4_xA4)
-      //set target link speed
-      Store (ShiftLeft (Add( Arg0, 2), 3), Local0)
-      procPciDwordRMW (Local0, 0x88, Not (0x0000002f), varDxF0x88)
-
-      // Determine port PCI address and check port present
-      Store (ShiftLeft (Add( Arg0, 2), 3), Local0)
-      And (procPciDwordRead (Local0, 0x70), 0x400000, varPortPresent)
-      if (LNotEqual (varPortPresent, 0)) {
-        //Disable ASPM on EP
-        if (LNotEqual (Arg0, 6)) {
-          Store (procPciDwordRead (Local0, 0x18), Local3)
-          Store (And (ShiftRight (Local3, 8), 0xFF),  varSecondaryBus)
-            Store ("Disable EP ASPM on SecondaryBus = ", Debug)
-            Store (varSecondaryBus, Debug)
-          Store (ShiftLeft (varSecondaryBus, 8), Local3)
-          Store (procPciDwordRead (Local3, 0xC), Local3)
-          Store (And (ShiftRight (Local3, 16), 0xFF),  varHeaderType)
-            Store ("Header Type = ", Debug)
-            Store (varHeaderType, Debug)
-          
-          if (LNotEqual (And (varHeaderType, 0x80), 0)) {
-            Store (0x7, varMultiFunction)
-          }
-          
-          Store (ShiftLeft (varSecondaryBus, 8), Local3)
-          Store (0, Local2)
-          while (LLessEqual (Local2, varMultiFunction)) {
-            
-            //Find PcieLinkControl register offset = PcieCapPtr + 0x10
-            Store (procFindPciCapability (Local3, 0x10), varPcieLinkControlOffset)
-            if (LNotEqual (varPcieLinkControlOffset, 0)) {
-              Add (varPcieLinkControlOffset, 0x10, varPcieLinkControlOffset)
-                
-                Store ("Function number of SecondaryBus = ", Debug)
-                Store (Local2, Debug)
-                Store ("Find PcieLinkControl register offset = ", Debug)
-                Store (varPcieLinkControlOffset, Debug)
-              // Save ASPM on EP
-              Store (procPciDwordRead (Local3, varPcieLinkControlOffset), varPcieLinkControlData)
-              Store (And (varPcieLinkControlData, 0x3), Index (varPcieLinkControlArray, Local2))
-              Store ("PcieLinkControlData = ", Debug)
-              Store (varPcieLinkControlData, Debug)
-              Store ("Save ASPM = ", Debug)
-              Store (DerefOf (Index (varPcieLinkControlArray, Local2)), Debug)
-              // Disable ASPM
-              if (LNotEqual (And (varPcieLinkControlData, 0x3), 0x0)) {
-                procPciDwordRMW (Local3, varPcieLinkControlOffset, Not (0x00000003), 0x00)
-                Store ("Disable ASPM on EP Complete!!", Debug)
-              }
-            }
-            Increment (Local2)
-            Increment (Local3)
-          }
+      Store (Concatenate ("  Function number of Secondary Bus : ", ToHexString (varFunctionLocal4), varStringBuffer), Debug)
+      Store (Concatenate ("  PcieLinkControl register offset  : ", ToHexString (varPcieLinkControlOffset), varStringBuffer), Debug)
+      // Save ASPM on EP
+      Store (procPciDwordRead (Add (varEndpointBdfLocal2, varFunctionLocal4) , varPcieLinkControlOffset), varPcieLinkControlData)
+      Store (And (varPcieLinkControlData, 0x3), Index (varPcieLinkControlArray, varFunctionLocal4))
 
-        } else {
+      Store (Concatenate ("  PcieLinkControl Data : ", ToHexString (varPcieLinkControlData), varStringBuffer), Debug)
 
-        	Store (procPcieSbAspmControl (0, 0), varAXCFGx68_PmCtrl)
-        	And (varAXCFGx68_PmCtrl, 0x3, Local1)
-  	      if (LNotEqual (Local1, 0x0)) {
-            procPcieSbAspmControl (0, 1)
-          }
-        }
-          Store (1, Local2)
-          while (Local2) {
-            //retrain port
-            procPciDwordRMW (Local0, 0x68, Not (0x00000000), 0x20)
-            Sleep (30)
-            while (And (procPciDwordRead (Local0, 0x68), 0x08000000)) {Sleep (10)}
-            Store (0, Local2)
-            if (LEqual (Arg1, 0x2)) {  // if Gen1
-              Store (procPciePortIndirectRegisterRead (Arg0, 0xA4), varLcCurrentDataRate)
-              if (LNotEqual (And (varLcCurrentDataRate, 0x800), 0)) {
-                Store (1, Local2)
-              }
-            }
-          }
-        //restore ASPM setting
-        if (LNotEqual (Arg0, 6)) {
-        	// Restore EP
-          //if (LNotEqual (varPcieLinkControlOffset, 0)) {
-          //  procPciDwordWrite (Local3, varPcieLinkControlOffset, varPcieLinkControlData)
-          //}
-          Store (ShiftLeft (varSecondaryBus, 8), Local3)
-          Store (0, Local2)
-          while (LLessEqual (Local2, varMultiFunction)) {
-            
-            //Find PcieLinkControl register offset = PcieCapPtr + 0x10
-            Store (procFindPciCapability (Local3, 0x10), varPcieLinkControlOffset)
-            if (LNotEqual (varPcieLinkControlOffset, 0)) {
-              Add (varPcieLinkControlOffset, 0x10, varPcieLinkControlOffset)
-                
-                Store ("Restore Function number of SecondaryBus = ", Debug)
-                Store (Local2, Debug)
-                Store ("Restore Find PcieLinkControl register offset = ", Debug)
-                Store (varPcieLinkControlOffset, Debug)
-                Store ("Restore ASPM = ", Debug)
-                Store (DerefOf (Index (varPcieLinkControlArray, Local2)), Debug)
-              procPciDwordWrite (Local3, varPcieLinkControlOffset, DerefOf (Index (varPcieLinkControlArray, Local2)))
-            }
-            Increment (Local2)
-            Increment (Local3)
-          }
+      procPciDwordRMW (Add (varEndpointBdfLocal2, varFunctionLocal4), varPcieLinkControlOffset, Not (0x00000003), 0x00)
+      Store ("Disable ASPM on EP Complete!!", Debug)
+      Increment (varFunctionLocal4)
+    }
+  }
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Restore ASPM
+   *
+   *  Arg0 - Port Index
+   */
+   Method (procRestoreAspm, 1, Serialized) {
 
-        } else {
-        	// Restore SB
-          procPcieSbAspmControl (varAXCFGx68_PmCtrl, 1)
-        }
-      } else {
-        Store ("  Device not present. Set capability and speed only", Debug)
-      }
-      //If request for UMI hide port congig space
-      if (LEqual (Arg0, 6)) {
-        procIndirectRegisterRMW (0x0, 0x60, 0x80, Not (0x40), 0x00);
+    Store (0, varPcieLinkControlOffset)
+    Store (0, varPcieLinkControlData)
+
+
+    // Restore SB ASPM
+    if (LEqual (Arg0, 6)) {
+      Store ("  Restore SB ASPM", Debug)
+      Store (Concatenate ("  PcieLinkControl Data : ", ToHexString (DerefOf(Index (varPcieLinkControlArray, 0))), varStringBuffer), Debug)
+      procPcieSbAspmControl (DerefOf(Index (varPcieLinkControlArray, 0)), 1)
+      return (0)
+    }
+    Store (ShiftLeft (Add( Arg0, 2), 3), varPortBdfLocal1)
+    // Restore EP ASPM
+    Store (procPciDwordRead (varPortBdfLocal1, 0x18), varTempLocal3)
+    Store (And (ShiftRight (varTempLocal3, 8), 0xFF),  varTempLocal3)
+
+    Store (Concatenate ("  Disable EP ASPM on SecondaryBus : ", ToHexString (varTempLocal3), varStringBuffer), Debug)
+
+    Store (ShiftLeft (varTempLocal3, 8), varEndpointBdfLocal2)
+    Store (procPciDwordRead (varEndpointBdfLocal2, 0xC), varTempLocal3)
+    Store (And (ShiftRight (varTempLocal3, 16), 0xFF),  varTempLocal3)
+
+    Store (Concatenate ("  EP Header type : ", ToHexString (varTempLocal3), varStringBuffer), Debug)
+
+    if (LNotEqual (And (varTempLocal3, 0x80), 0)) {
+      Store (0x7, varMaxFunctionLocal0)
+    } else {
+      Store (0x0, varMaxFunctionLocal0)
+    }
+    Store (0, varFunctionLocal4)
+    while (LLessEqual (varFunctionLocal4, varMaxFunctionLocal0)) {
+      //Find PcieLinkControl register offset = PcieCapPtr + 0x10
+      Store (procFindPciCapability (Add (varEndpointBdfLocal2, varFunctionLocal4), 0x10), varPcieLinkControlOffset)
+      if (LEqual (varPcieLinkControlOffset, 0)) {
+        Increment (varFunctionLocal4)
+        continue
       }
+      Add (varPcieLinkControlOffset, 0x10, varPcieLinkControlOffset)
+
+      Store (Concatenate ("  Restore Function number of SecondaryBus : ", ToHexString (varFunctionLocal4), varStringBuffer), Debug)
+      Store (Concatenate ("  Restore PcieLinkControl register offset : ", ToHexString (varPcieLinkControlOffset), varStringBuffer), Debug)
+      Store (Concatenate ("  PcieLinkControl Data : ", ToHexString (DerefOf (Index (varPcieLinkControlArray, varFunctionLocal4))), varStringBuffer), Debug)
+
+      procPciDwordWrite (Add (varEndpointBdfLocal2, varFunctionLocal4), varPcieLinkControlOffset, DerefOf (Index (varPcieLinkControlArray, varFunctionLocal4)))
+      Increment (varFunctionLocal4)
+    }
+  }
+
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *   Request VID
+   *
+   *  Arg0 - Port Index
+   *  Arg1 - PCIe speed
+   */
+
+  Method (procPcieSetLinkSpeed, 2) {
+    Store (ShiftLeft (Add( Arg0, 2), 3), Local0)
+    if (LEqual (Arg1, DEF_LINK_SPEED_GEN1)) {
+      procPciDwordRMW (Local0, 0x88, Not (0x0000002f), 0x21)
+      procPciePortIndirectRegisterRMW (Arg0, 0xA4, Not (0x20000001), 0x0)
+    } else {
+      procPciePortIndirectRegisterRMW (Arg0, 0xA4, Not (0x20000001), 0x20000001)
+      procPciDwordRMW (Local0, 0x88, Not (0x0000002f), 0x2)
     }
-    Store ("SetPortCapabilityAndSpeed Exit", Debug)
   }
-  Mutex (varVoltageChangeMutex, 0)
+
+
   /*----------------------------------------------------------------------------------------*/
   /**
    *   Request VID
@@ -636,18 +725,17 @@
    *  Arg1 - 0 = do not wait intil voltage is set
    *         1 = wait until voltage is set
    */
-  Method (procPcieSetVoltage, 2) {
-    Store ("PcieSetVoltage(procPcieSetVoltage) Enter", Debug)
-    Acquire(varVoltageChangeMutex, 0xFFFF)
+  Method (procPcieSetVoltage, 2, Serialized) {
+    Store ("PcieSetVoltage Enter", Debug)
     Store (procIndirectRegisterRead (0x0, 0x60, 0xEA), Local1)
     //Enable voltage change
     Or (Local1, 0x2, Local1)
     procIndirectRegisterWrite (0x0, 0x60, 0xEA, Local1)
     //Clear voltage index
     And (Local1, Not (ShiftLeft (0x3, 3)), Local1)
+
+    Store (Concatenate ("  Voltage Index:", ToHexString (Arg0), Local6), Debug)
     //Set new voltage index
-    Store ("  Voltage Index:", Debug)
-    Store (Arg0, Debug)
     Or (Local1, ShiftLeft (Arg0, 3), Local1)
     //Togle request
     And (Not (Local1), 0x4, Local2)
@@ -658,8 +746,7 @@
         And (procIndirectRegisterRead (0x0, 0x60, 0xEB), 0x1, Local1)
       }
     }
-    Release (varVoltageChangeMutex)
-    Store ("PcieSetVoltage(procPcieSetVoltage) Exit", Debug)
+    Store ("PcieSetVoltage Exit", Debug)
   }
 
   /*----------------------------------------------------------------------------------------*/
@@ -680,3 +767,14 @@
     }
   }
 
+  /*----------------------------------------------------------------------------------------*/
+  /**
+   *  Read PCIe port indirect register
+   *
+   *  Arg0 - Ref Source Pckage
+   *  Arg1 - Ref to Destination Package
+   *
+   */
+  Method (procPsppGetAcDcState, 0 , NotSerialized) {
+    Return (And (varPsppAcDcState, varPsppAcDcOverride))
+  }