Pstate: VDM Enablement

- SGPE stop exit and entry changes to set-up the QPPM VDM registers
- CME stop exit and entry changes to enable/disable VDMs during Stop
  operations
- CME code to change VDM VID compare and thresholds during Pstate operations
- Compile-time flags to optionally remove Resclk/VDM code (due to size
  constraints)

Change-Id: I1fe8b2231c554b4f181472efab1f5cc45cc087d6
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/40676
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Reviewed-by: Michael S. Floyd <mfloyd@us.ibm.com>
Reviewed-by: BRIAN D. VICTOR <brian.d.victor1@ibm.com>
Reviewed-by: Gregory S. Still <stillgs@us.ibm.com>

7 files changed, 372 insertions, 36 deletions

diff --git a/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h b/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h
index 919f01e3..b9fff80c 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h
@@ -57,6 +57,8 @@
     #define HW405292_NDD1_PCBMUX_SAVIOR         1
     #define MASK_MSR_SEM6
     #define RUN_NDD1_ABIST_IN_PARALLEL_MODE     1
+    #define USE_CME_VDM_FEATURE
+    #undef  USE_CME_RESCLK_FEATURE
 #endif
 
 #if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
@@ -65,6 +67,8 @@
     #undef  SKIP_EXIT_CATCHUP
     #define SKIP_ENTRY_CATCHUP 1
     #define SKIP_EXIT_CATCHUP  1
+    #define USE_CME_VDM_FEATURE
+    #define USE_CME_RESCLK_FEATURE
 #endif
 
 // --------------------
diff --git a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.c b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.c
index 66944709..c172d0b0 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.c
@@ -59,6 +59,11 @@ cmeHeader_t* G_cmeHeader;
 LocalPstateParmBlock* G_lppb;
 extern CmePstateRecord G_cme_pstate_record;
 
+const uint8_t G_vdm_threshold_table[13] =
+{
+    0x00, 0x01, 0x03, 0x02, 0x06, 0x07, 0x05, 0x04,
+    0x0C, 0x0D, 0x0F, 0x0E, 0x0A
+};
 
 //
 //send_pig_packet
@@ -180,6 +185,7 @@ void intercme_msg_recv(uint32_t* msg, INTERCME_MSG_TYPE type)
     out32(CME_LCL_EISR_CLR, BIT32(29));
 }
 
+#ifdef USE_CME_RESCLK_FEATURE
 void p9_cme_resclk_get_index(uint32_t pstate, uint32_t* resclk_index)
 {
     int32_t i = RESCLK_FREQ_REGIONS;
@@ -193,10 +199,13 @@ void p9_cme_resclk_get_index(uint32_t pstate, uint32_t* resclk_index)
     PK_TRACE_DBG("resclk_idx[i=%d]=%d", i, G_lppb->resclk.resclk_index[i]);
     *resclk_index = (uint32_t)G_lppb->resclk.resclk_index[i];
 }
+#endif//USE_CME_RESCLK_FEATURE
 
 void p9_cme_analog_control(uint32_t core_mask, ANALOG_CONTROL enable)
 {
-    if((in32(CME_LCL_FLAGS)) & BIT32(CME_FLAGS_RCLK_OPERABLE))
+#ifdef USE_CME_RESCLK_FEATURE
+
+    if(in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_RCLK_OPERABLE))
     {
         uint32_t pstate;
         uint32_t curr_idx;
@@ -260,8 +269,148 @@ void p9_cme_analog_control(uint32_t core_mask, ANALOG_CONTROL enable)
             p9_cme_resclk_update(core_mask, pstate, curr_idx);
         }
     }
+
+#endif//USE_CME_RESCLK_FEATURE
+
+#ifdef USE_CME_VDM_FEATURE
+
+    if(in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_VDM_OPERABLE))
+    {
+        if(enable)
+        {
+            PK_TRACE_INF("vdm | enabling vdms");
+            // Clear Disable (Poweron is set earlier in Exit flow)
+            CME_PUTSCOM(PPM_VDMCR_CLR, core_mask, BIT64(1));
+        }
+        else
+        {
+            PK_TRACE_INF("vdm | disabling vdms");
+            // Clear Poweron and set Disable in one operation
+            CME_PUTSCOM(PPM_VDMCR, core_mask, BIT64(1));
+        }
+    }
+
+#endif//USE_CME_VDM_FEATURE
+}
+
+#ifdef USE_CME_VDM_FEATURE
+uint32_t pstate_to_vpd_region(uint32_t pstate)
+{
+    if(pstate > G_lppb->operating_points[NOMINAL].pstate)
+    {
+        return REGION_POWERSAVE_NOMINAL;
+    }
+    else if(pstate > G_lppb->operating_points[TURBO].pstate)
+    {
+        return REGION_NOMINAL_TURBO;
+    }
+    else
+    {
+        return REGION_TURBO_ULTRA;
+    }
+}
+
+uint32_t pstate_to_vid_compare(uint32_t pstate, uint32_t region)
+{
+    return((((uint32_t)G_lppb->PsVIDCompSlopes[region]
+             * ((uint32_t)G_lppb->operating_points[region].pstate - pstate)
+             + VDM_VID_COMP_ADJUST) >> VID_SLOPE_FP_SHIFT_12)
+           + (uint32_t)G_lppb->vid_point_set[region]);
+}
+
+void calc_vdm_threshold_indices(uint32_t pstate, uint32_t region,
+                                uint32_t indices[])
+{
+    static VDM_ROUNDING_ADJUST vdm_rounding_adjust[NUM_THRESHOLD_POINTS] =
+    {
+        VDM_OVERVOLT_ADJUST,
+        VDM_SMALL_ADJUST,
+        VDM_LARGE_ADJUST,
+        VDM_XTREME_ADJUST
+    };
+
+    uint32_t i = 0;
+    int32_t psdiff = (uint32_t)G_lppb->operating_points[region].pstate - pstate;
+
+    for(i = 0; i < NUM_THRESHOLD_POINTS; ++i)
+    {
+        // Cast every math term into 32b for more efficient PPE maths
+        indices[i] = (uint32_t)((int32_t)G_lppb->threshold_set[region][i]
+                                + (((int32_t)G_lppb->PsVDMThreshSlopes[region][i] * psdiff
+                                    // Apply the rounding adjust
+                                    + (int32_t)vdm_rounding_adjust[i]) >> THRESH_SLOPE_FP_SHIFT));
+
+    }
+
+    // Check the interpolation result; since each threshold has a distinct round
+    // adjust, the calculated index can be invalid relative to another threshold
+    // index. Overvolt does not need to be checked and Small Droop will always
+    // be either 0 or greater than 0 by definition.
+    // Ensure that small <= large <= xtreme; where any can be == 0.
+    indices[VDM_LARGE_IDX] = ((indices[VDM_LARGE_IDX] < indices[VDM_SMALL_IDX])
+                              && (indices[VDM_LARGE_IDX] != 0))
+                             ? indices[VDM_SMALL_IDX] : indices[VDM_LARGE_IDX];
+    indices[VDM_XTREME_IDX] = ((indices[VDM_XTREME_IDX] < indices[VDM_LARGE_IDX])
+                               && (indices[VDM_XTREME_IDX] != 0))
+                              ? indices[VDM_LARGE_IDX] : indices[VDM_XTREME_IDX];
+    indices[VDM_XTREME_IDX] = ((indices[VDM_XTREME_IDX] < indices[VDM_SMALL_IDX])
+                               && (indices[VDM_LARGE_IDX]  == 0)
+                               && (indices[VDM_XTREME_IDX] != 0))
+                              ? indices[VDM_SMALL_IDX] : indices[VDM_XTREME_IDX];
+}
+
+void p9_cme_vdm_update(uint32_t pstate)
+{
+    uint32_t new_idx[NUM_THRESHOLD_POINTS] = { 0 };
+    uint32_t i = 0;
+    uint32_t not_done = BITS32(32 - NUM_THRESHOLD_POINTS, NUM_THRESHOLD_POINTS);
+    uint64_t scom_data = 0;
+    uint64_t base_scom_data = 0;
+    uint32_t region = pstate_to_vpd_region(pstate);
+
+    // Calculate the new index for each threshold
+    calc_vdm_threshold_indices(pstate, region, new_idx);
+
+    // Look-up the VID compare value using the Pstate
+    base_scom_data |= (uint64_t)(pstate_to_vid_compare(pstate, region)
+                                 & BITS32(24, 8)) << 56;
+
+    // Step all thresholds in parallel until each reaches its new target.
+    // Doing this in parallel minimizes the number of interppm scom writes.
+    do
+    {
+        // Only keep the VID compare value
+        scom_data = base_scom_data;
+
+        // Loop over all 4 thresholds (overvolt, small, large, xtreme)
+        for(i = 0; i < NUM_THRESHOLD_POINTS; ++i)
+        {
+            if(new_idx[i] != G_cme_pstate_record.vdmData.vdm_threshold_idx[i])
+            {
+                // Decrement or increment the current index, whichever is
+                // required to reach the new/target index
+                G_cme_pstate_record.vdmData.vdm_threshold_idx[i] +=
+                    (G_cme_pstate_record.vdmData.vdm_threshold_idx[i] < new_idx[i])
+                    ? 1 : -1;
+            }
+            else
+            {
+                not_done &= 0x1 << i;
+            }
+
+            // OR the new threshold greycode into the correct position
+            scom_data |= (uint64_t)G_vdm_threshold_table[
+                             G_cme_pstate_record.vdmData.vdm_threshold_idx[i]]
+                         << (52 - (i * 4));
+        }
+
+        ippm_write(QPPM_VDMCFGR, scom_data);
+    }
+    while(not_done);
 }
+#endif//USE_CME_VDM_FEATURE
 
+#ifdef USE_CME_RESCLK_FEATURE
 void p9_cme_resclk_update(ANALOG_TARGET target, uint32_t pstate, uint32_t curr_idx)
 {
     uint64_t base_val;
@@ -337,6 +486,7 @@ void p9_cme_resclk_update(ANALOG_TARGET target, uint32_t pstate, uint32_t curr_i
         }
     }
 }
+#endif//USE_CME_RESCLK_FEATURE
 
 void p9_cme_pstate_pmsr_updt(uint32_t coreMask)
 {
diff --git a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.h b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.h
index 1090eb00..b051c220 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.h
@@ -66,6 +66,11 @@ typedef struct
     uint32_t common_resclk_idx;
 } cme_resclk_data_t;
 
+typedef struct
+{
+    uint32_t vdm_threshold_idx[NUM_THRESHOLD_POINTS];
+} cme_vdm_data_t;
+
 typedef enum
 {
     ANALOG_CORE0  = (uint32_t)CME_MASK_C0,
@@ -79,6 +84,38 @@ typedef enum
 
 typedef enum
 {
+    VDM_OVERVOLT_IDX = 0,
+    VDM_SMALL_IDX    = 1,
+    VDM_LARGE_IDX    = 2,
+    VDM_XTREME_IDX   = 3
+} VDM_THRESHOLD_IDX;
+
+typedef enum
+{
+    // VDM_OVERVOLT_ADJUST
+    // 4/8 rounding (8mV resolution so +/- 4 mV error)
+    // yields 3/7 aggressive and 4/7 conservative to slightly favor not
+    // increasing to Fmax as often
+    VDM_OVERVOLT_ADJUST = (uint32_t)((1 << THRESH_SLOPE_FP_SHIFT) * ((float)4 / 8)),
+    // VDM_SMALL_ADJUST
+    // 5/8 rounding (8mV resolution so +5/-3 mV error)
+    // yields 2/7 conservative and 5/7 aggressive to favor protecting against
+    // performance loss
+    VDM_SMALL_ADJUST    = (uint32_t)((1 << THRESH_SLOPE_FP_SHIFT) * ((float)5 / 8)),
+    // VDM_LARGE_ADJUST and VDM_XTREME_ADJUST
+    // 2/8 rounding (8mV resolution so +3/-5 mV error)
+    // yields 2/7 aggressive and 5/7 conservative to favor protecting droop
+    // guardband
+    VDM_LARGE_ADJUST    = (uint32_t)((1 << THRESH_SLOPE_FP_SHIFT) * ((float)2 / 8)),
+    VDM_XTREME_ADJUST   = (uint32_t)((1 << THRESH_SLOPE_FP_SHIFT) * ((float)2 / 8)),
+    //VDM_VID_COMP_ADJUST
+    // 2/4 rounding (4mV resolution so +/- 2mV error)
+    // yields 1/3 (1mV) aggressive and 2/3 (1 or 2mV) conservative answer
+    VDM_VID_COMP_ADJUST = (uint32_t)((1 << VID_SLOPE_FP_SHIFT_12) * ((float)2 / 4))
+} VDM_ROUNDING_ADJUST;
+
+typedef enum
+{
     ANALOG_DISABLE = (uint32_t)0,
     ANALOG_ENABLE  = (uint32_t)1
 } ANALOG_CONTROL;
@@ -104,7 +141,12 @@ typedef struct
     uint32_t globalPstate;
     uint32_t cmeMaskGoodCore;
     uint32_t pmcrSeenErr;
+#ifdef USE_CME_RESCLK_FEATURE
     cme_resclk_data_t resclkData;
+#endif//USE_CME_RESCLK_FEATURE
+#ifdef USE_CME_VDM_FEATURE
+    cme_vdm_data_t vdmData;
+#endif//USE_CME_VDM_FEATURE
 } CmePstateRecord;
 
 typedef struct
@@ -112,7 +154,6 @@ typedef struct
     uint32_t seqNum;
 } cme_pstate_pmcr_data_t;
 
-
 void p9_cme_pstate_pmcr_thread(void*);
 void p9_cme_pstate_db_thread(void*);
 void p9_cme_pstate_pmcr_handler(void*, PkIrqId);
@@ -124,10 +165,18 @@ void ippm_read(uint32_t addr, uint64_t* data);
 void ippm_write(uint32_t addr, uint64_t data);
 void intercme_msg_send(uint32_t msg, INTERCME_MSG_TYPE type);
 void intercme_msg_recv(uint32_t* msg, INTERCME_MSG_TYPE type);
-void p9_cme_resclk_get_index(uint32_t pstate, uint32_t* resclk_index);
 void p9_cme_analog_control(uint32_t core_mask, ANALOG_CONTROL enable);
-void p9_cme_resclk_update(ANALOG_TARGET target, uint32_t pstate, uint32_t curr_idx);
 void p9_cme_pstate_pmsr_updt(uint32_t coreMask);
-
+#ifdef USE_CME_RESCLK_FEATURE
+    void p9_cme_resclk_get_index(uint32_t pstate, uint32_t* resclk_index);
+    void p9_cme_resclk_update(ANALOG_TARGET target, uint32_t pstate, uint32_t curr_idx);
+#endif//USE_CME_RESCLK_FEATURE
+#ifdef USE_CME_VDM_FEATURE
+void p9_cme_vdm_update(uint32_t pstate);
+uint32_t pstate_to_vid_compare(uint32_t pstate, uint32_t region);
+uint32_t pstate_to_vpd_region(uint32_t pstate);
+void calc_vdm_threshold_indices(uint32_t pstate, uint32_t region,
+                                uint32_t indices[]);
+#endif//USE_CME_VDM_FEATURE
 
 #endif //_P9_CME_PSTATE_H_
diff --git a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_thread_db.c b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_thread_db.c
index e0e9ccd1..63c7743c 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_thread_db.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_thread_db.c
@@ -57,6 +57,7 @@ extern LocalPstateParmBlock* G_lppb;
 uint32_t G_cme_flags;
 cppm_cmedb0_t G_dbData;
 uint32_t G_next_pstate;
+extern uint8_t G_vdm_threshold_table[];
 
 //
 //Function Prototypes
@@ -145,6 +146,42 @@ void p9_cme_pstate_db_thread(void* arg)
                                          - (uint32_t)((scom_data & BITS64(1, 11)) >> SHIFT64(11));
         PK_TRACE_INF("qm | initial pstate=%d", G_cme_pstate_record.quadPstate);
 
+#ifdef USE_CME_VDM_FEATURE
+
+        // VDM Enablement (QM)
+        // Do this prior to synchronizing the Pstate w/ its Sibling CME
+        if(G_cme_pstate_record.qmFlag
+           && (G_cmeHeader->g_cme_qm_mode_flags & CME_QM_FLAG_SYS_VDM_ENABLE))
+        {
+            uint32_t i;
+            uint32_t region = pstate_to_vpd_region(G_cme_pstate_record.quadPstate);
+            // VID compare
+            scom_data = (uint64_t)(pstate_to_vid_compare(G_cme_pstate_record.quadPstate, region)
+                                   & BITS32(24, 8)) << 56;
+            // Calculate the new index for each threshold
+            calc_vdm_threshold_indices(G_cme_pstate_record.quadPstate, region,
+                                       G_cme_pstate_record.vdmData.vdm_threshold_idx);
+
+            // Thresholds; no stepping is required since the VDMs are still
+            // disabled at this point.
+            for(i = 0; i < NUM_THRESHOLD_POINTS; ++i)
+            {
+                scom_data |= (uint64_t)G_vdm_threshold_table[
+                                 G_cme_pstate_record.vdmData.vdm_threshold_idx[i]]
+                             << (52 - (i * 4));
+            }
+
+            PK_TRACE_INF("qm | initial vdmcfgr=%08x%08x", scom_data.upper,
+                         scom_data.lower);
+            ippm_write(QPPM_VDMCFGR, scom_data);
+            // Assumes 100us has elapsed during cache chiplet wakeup after
+            // enabling the full-speed cache clock grid
+            // Clear VDM Disable
+            ippm_write(PPM_VDMCR_CLR, BIT64(1));
+        }
+
+#endif//USE_CME_VDM_FEATURE
+
         // Synchronize initial pstate w/ sibling CME
         if(G_cme_pstate_record.siblingCMEFlag)
         {
@@ -169,7 +206,7 @@ void p9_cme_pstate_db_thread(void* arg)
             cores |= CME_MASK_C1;
         }
 
-        // Resonant Clocking Initialization (Sibling)
+        // Pstate Initialization (Sibling)
         out32(CME_LCL_EITR_OR, BIT32(29));
         out32(CME_LCL_EIPR_OR, BIT32(29));
 
@@ -204,6 +241,8 @@ void p9_cme_pstate_db_thread(void* arg)
         PK_PANIC(CME_PSTATE_RESCLK_ENABLED_AT_BOOT);
     }
 
+#ifdef USE_CME_RESCLK_FEATURE
+
     // Resonant Clocking Initialization (QM + Sibling)
     if(G_cmeHeader->g_cme_qm_mode_flags & CME_QM_FLAG_RESCLK_ENABLE)
     {
@@ -239,6 +278,19 @@ void p9_cme_pstate_db_thread(void* arg)
         out32(CME_LCL_FLAGS_OR, BIT32(CME_FLAGS_RCLK_OPERABLE));
     }
 
+#endif//USE_CME_RESCLK_FEATURE
+
+#ifdef USE_CME_VDM_FEATURE
+
+    // Set VDM Operable flag for both QM and Sib, at this point the QM has
+    // completed all initialization for VDMs and QM and Sib are interlocked
+    if(G_cmeHeader->g_cme_qm_mode_flags & CME_QM_FLAG_SYS_VDM_ENABLE)
+    {
+        out32(CME_LCL_FLAGS_OR, BIT32(CME_FLAGS_VDM_OPERABLE));
+    }
+
+#endif//USE_CME_VDM_FEATURE
+
     //Doorbell Thread(this thread) will continue to run on
     //Quad Manager CME. The sibling CME has intercme_in0 enabled
     //and won't run this thread past this point.
@@ -423,8 +475,6 @@ inline void p9_cme_pstate_db0_suspend()
     g_eimr_override |= BITS64(34, 2);
 
     p9_cme_pstate_notify_sib(); //Notify sibling
-    // Prevent Resclk, VDM updates
-    out32(CME_LCL_FLAGS_CLR, (BIT32(CME_FLAGS_RCLK_OPERABLE) | BIT32(CME_FLAGS_VDM_OPERABLE)));
 
     //Send type4(ack doorbell)
     ppmPigData.value = 0;
@@ -499,6 +549,34 @@ inline void p9_cme_pstate_freq_update()
         }
 
         ippm_write(QPPM_DPLL_FREQ, dpllFreq.value);
+
+        if(in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_VDM_OPERABLE))
+        {
+            // TODO DPLL Mode 3.5 will require some other stuffs
+            data64_t scom_data = { 0 };
+
+            PK_TRACE_INF("Poll on DPLL_STAT[update_complete]");
+            // ... to indicate that the DPLL has sampled the newly requested
+            // frequency into its internal registers as a target,
+            // but may not yet be there
+
+            do
+            {
+                ippm_read(QPPM_DPLL_STAT, &scom_data.value);
+            }
+            while(!(scom_data.words.lower & BIT32(28)));
+
+            PK_TRACE_INF("Poll on DPLL_STAT[block_active|lock]");
+            // ... to indicate that the DPLL is safely either at the new frequency
+            // or in droop protection below the new frequency
+
+            do
+            {
+                ippm_read(QPPM_DPLL_STAT, &scom_data.value);
+            }
+            while(!(scom_data.words.lower & BITS32(30, 2)));
+        }
+
         PK_TRACE_INF("DB_TH: Freq Updt Exit\n");
     }
 }
@@ -507,24 +585,33 @@ void p9_cme_pstate_update()
 {
     PK_TRACE_INF("DB_TH: Pstate Updt Enter");
 
-    if(G_cme_pstate_record.siblingCMEFlag)
+    G_next_pstate = (G_dbData.value >> (in32(CME_LCL_SRTCH0) &
+                                        (BITS32(CME_SCRATCH_LOCAL_PSTATE_IDX_START,
+                                                CME_SCRATCH_LOCAL_PSTATE_IDX_LENGTH)))) & 0xFF;
+
+    if(G_next_pstate != G_cme_pstate_record.quadPstate)
     {
-        // "Lock" the sibling until the pstate transition is complete
-        intercme_msg_send(0, IMT_LOCK_SIBLING);
-    }
+        if(G_cme_pstate_record.siblingCMEFlag)
+        {
+            // "Lock" the sibling until the pstate transition is complete
+            intercme_msg_send(0, IMT_LOCK_SIBLING);
+            // The Sibling is a "pumpkin" from this point forward until calling
+            // p9_cme_pstate_notify_sib()
+        }
 
-    G_next_pstate = (G_dbData.value >>
-                     (in32(CME_LCL_SRTCH0) &
-                      (BITS32(CME_SCRATCH_LOCAL_PSTATE_IDX_START, CME_SCRATCH_LOCAL_PSTATE_IDX_LENGTH))
-                     )) & 0xFF;
+        G_cme_pstate_record.globalPstate = (G_dbData.value & BITS64(8, 8))
+                                           >> SHIFT64(15);
 
-    G_cme_pstate_record.globalPstate = (G_dbData.value & BITS64(8, 8)) >> SHIFT64(15);
+        PK_TRACE_INF("DB_TH: DBData=0x%08x%08x\n", G_dbData.value >> 32,
+                     G_dbData.value);
 
-    PK_TRACE_INF("DB_TH: DBData=0x%08x%08x\n", G_dbData.value >> 32, G_dbData.value);
+        // Dropping frequency: freq update, resclk, vdms, then ivrms (TODO)
+        if(G_next_pstate > G_cme_pstate_record.quadPstate)
+        {
+            p9_cme_pstate_freq_update();
+        }
 
-    if(G_next_pstate > G_cme_pstate_record.quadPstate)
-    {
-        p9_cme_pstate_freq_update();
+#ifdef USE_CME_RESCLK_FEATURE
 
         if(G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
         {
@@ -536,27 +623,28 @@ void p9_cme_pstate_update()
 
             pk_critical_section_exit(&ctx);
         }
-    }
-    else if(G_next_pstate < G_cme_pstate_record.quadPstate)
-    {
-        if(G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
-        {
-            PkMachineContext ctx;
-            pk_critical_section_enter(&ctx);
 
-            p9_cme_resclk_update(ANALOG_COMMON, G_next_pstate,
-                                 G_cme_pstate_record.resclkData.common_resclk_idx);
+#endif//USE_CME_RESCLK_FEATURE
+#ifdef USE_CME_VDM_FEATURE
 
-            pk_critical_section_exit(&ctx);
+        if(G_cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+        {
+            p9_cme_vdm_update(G_next_pstate);
         }
 
-        p9_cme_pstate_freq_update();
-    }
+#endif//USE_CME_VDM_FEATURE
 
-    p9_cme_pstate_notify_sib(); //Notify sibling
-    G_cme_pstate_record.quadPstate = G_next_pstate;//Must Update quadPstate before calling pmsr_updt
-    p9_cme_pstate_pmsr_updt(G_cme_record.core_enabled);
+        // Raising frequency: ivrm (TODO), vdm, resclk, freq update
+        if(G_next_pstate < G_cme_pstate_record.quadPstate)
+        {
+            p9_cme_pstate_freq_update();
+        }
 
+        p9_cme_pstate_notify_sib(); // Release Sibling to change Pstate as well
+        // Must update quadPstate before calling PMSR update
+        G_cme_pstate_record.quadPstate = G_next_pstate;
+        p9_cme_pstate_pmsr_updt(G_cme_record.core_enabled);
+    }
 
     PK_TRACE_INF("DB_TH: Pstate Updt Exit");
 }
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c
index 8cc493f7..9b9a6ea4 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c
@@ -695,6 +695,13 @@ p9_cme_stop_exit()
 
                 // Note: in this case, no need to call p9_cme_pcbmux_savior_epilogue
 
+                if(in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_VDM_OPERABLE))
+                {
+                    // Poweron the VDM giving it time to powerup prior to enabling
+                    PK_TRACE_INF("Set Poweron bit in VDMCR");
+                    CME_PUTSCOM(PPM_VDMCR_OR, core, BIT64(0));
+                }
+
                 //========================
                 MARK_TAG(SX_POWERON, core)
                 //========================
diff --git a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_entry.c b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_entry.c
index 493ee657..20ff169b 100644
--- a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_entry.c
+++ b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_entry.c
@@ -66,6 +66,7 @@ p9_sgpe_stop_entry()
 #if !SKIP_IPC
     uint32_t      rc               = 0;
 #endif
+    sgpeHeader_t* pSgpeImgHdr     = (sgpeHeader_t*)(OCC_SRAM_SGPE_HEADER_ADDR);
 
     //--------------------------------------------------------------------------
     PK_TRACE("+++++ +++++ BEGIN OF STOP ENTRY +++++ +++++");
@@ -925,6 +926,18 @@ p9_sgpe_stop_entry()
         PK_TRACE("Checking status of Local Checkstop");
         GPE_GETSCOM(GPE_SCOM_ADDR_QUAD(EQ_LOCAL_XSTOP_ERR, qloop), local_xstop);
 
+        if(pSgpeImgHdr->g_sgpe_reserve_flags & SGPE_VDM_ENABLE_BIT_POS)
+        {
+            PK_TRACE("Clear Jump Protect Enable (no need to poll DPLL_STAT");
+            GPE_PUTSCOM(GPE_SCOM_ADDR_QUAD(EQ_QPPM_DPLL_CTRL_CLEAR, qloop), BIT64(1));
+
+            PK_TRACE("Write QPPM VDMCR to set Disable and clear Poweron");
+            GPE_PUTSCOM(GPE_SCOM_ADDR_QUAD(PPM_VDMCR, qloop), BIT64(1));
+
+            PK_TRACE("Clear QPPM VDMCFGR");
+            GPE_PUTSCOM(GPE_SCOM_ADDR_QUAD(QPPM_VDMCFGR, qloop), 0);
+        }
+
         //===========================
         MARK_TRAP(SE_STOP_CACHE_CLKS)
         //===========================
diff --git a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c
index 481ec8aa..d70f48ac 100644
--- a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c
+++ b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c
@@ -366,6 +366,7 @@ p9_sgpe_stop_exit()
 #if !SKIP_IPC
     uint32_t      rc              = 0;
 #endif
+    sgpeHeader_t* pSgpeImgHdr = (sgpeHeader_t*)(OCC_SRAM_SGPE_HEADER_ADDR);
 
     //===============================
     MARK_TAG(BEGINSCOPE_STOP_EXIT, 0)
@@ -632,6 +633,30 @@ p9_sgpe_stop_exit()
             PK_TRACE_INF("SX.11D: Cache Dpll Setup");
             p9_hcd_cache_dpll_setup(qloop);
 
+            if(pSgpeImgHdr->g_sgpe_reserve_flags & SGPE_VDM_ENABLE_BIT_POS)
+            {
+                // Note: 100us must elapse after starting full-speed cache
+                // clock gird and 10us after setting power-on (to VDM) before
+                // clearing the VDM disable in CME
+                PK_TRACE("Clear QPPM VDMCFGR");
+                GPE_PUTSCOM(GPE_SCOM_ADDR_QUAD(QPPM_VDMCFGR, qloop), 0);
+
+                PK_TRACE("Write QPPM VDMCR to set Disable and set Poweron");
+                GPE_PUTSCOM(GPE_SCOM_ADDR_QUAD(PPM_VDMCR, qloop), BITS64(0, 2));
+
+                PK_TRACE("Set Jump Protect Enable");
+                GPE_PUTSCOM(GPE_SCOM_ADDR_QUAD(EQ_QPPM_DPLL_CTRL_OR, qloop), BIT64(1));
+
+                PK_TRACE("Poll on DPLL_STAT[update_complete]");
+
+                do
+                {
+                    GPE_GETSCOM(GPE_SCOM_ADDR_QUAD(EQ_QPPM_DPLL_STAT, qloop),
+                                scom_data.value);
+                }
+                while(!(scom_data.words.lower & BIT32(28)));
+            }
+
 #if !SKIP_INITF
 
             PK_TRACE_DBG("Cache DCC Skewadjust Setup");