PM: Implement CME VDM Jump Values

Change-Id: Id0987b453958423187a7f7f5c0db210dab49b4f3
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/43157
Reviewed-by: BRIAN D. VICTOR <brian.d.victor1@ibm.com>
Reviewed-by: YUE DU <daviddu@us.ibm.com>
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Reviewed-by: Brian T. Vanderpool <vanderp@us.ibm.com>
Tested-by: FSP CI Jenkins <fsp-CI-jenkins+hostboot@us.ibm.com>
Reviewed-by: Gregory S. Still <stillgs@us.ibm.com>

4 files changed, 253 insertions, 48 deletions

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 ceb7e877..6af4b250 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
@@ -88,7 +88,24 @@ int send_pig_packet(uint64_t data, uint32_t coreMask)
     return rc;
 }
 
-void ippm_read(uint32_t addr, uint64_t* data)
+void poll_dpll_update_complete()
+{
+    data64_t polldata;
+    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, &polldata.value);
+    }
+    while(!(polldata.words.lower & BIT32(28)));
+}
+
+// Non-atomic Interppm-read, this function is not made availabe via the header
+// as the toplevel-wrapper (atomic) ippm_read should be used instead
+void nonatomic_ippm_read(uint32_t addr, uint64_t* data)
 {
     // G_cme_pstate_record.cmeMaskGoodCore MUST be set!
     uint64_t val;
@@ -118,7 +135,17 @@ void ippm_read(uint32_t addr, uint64_t* data)
     *data = val;
 }
 
-void ippm_write(uint32_t addr, uint64_t data)
+void ippm_read(uint32_t addr, uint64_t* data)
+{
+    PkMachineContext ctx __attribute__((unused));
+    pk_critical_section_enter(&ctx);
+    nonatomic_ippm_read(addr, data);
+    pk_critical_section_exit(&ctx);
+}
+
+// Non-atomic Interppm-write, this function is not made availabe via the header
+// as the toplevel-wrapper (atomic) ippm_write should be used instead
+void nonatomic_ippm_write(uint32_t addr, uint64_t data)
 {
     // G_cme_pstate_record.cmeMaskGoodCore MUST be set!
     uint64_t val;
@@ -146,6 +173,14 @@ void ippm_write(uint32_t addr, uint64_t data)
     }
 }
 
+void ippm_write(uint32_t addr, uint64_t data)
+{
+    PkMachineContext ctx __attribute__((unused));
+    pk_critical_section_enter(&ctx);
+    nonatomic_ippm_write(addr, data);
+    pk_critical_section_exit(&ctx);
+}
+
 void intercme_msg_send(uint32_t msg, INTERCME_MSG_TYPE type)
 {
     out32(CME_LCL_ICSR, (msg << 4) | type);
@@ -351,11 +386,126 @@ uint32_t pstate_to_vpd_region(uint32_t pstate)
 
 uint32_t pstate_to_vid_compare(uint32_t pstate, uint32_t region)
 {
+    // *INDENT-OFF*
     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]);
+             + (uint32_t)G_lppb->vid_point_set[region]);
+    // *INDENT-ON*
+}
+
+#if NIMBUS_DD_LEVEL != 10
+uint32_t calc_vdm_jump_values(uint32_t pstate, uint32_t region)
+{
+    static uint32_t vdm_jump_values[NUM_JUMP_VALUES] = { 0 };
+    uint32_t i = 0;
+    uint32_t new_jump_values = 0;
+    int32_t  psdiff = (uint32_t)G_lppb->operating_points[region].pstate - pstate;
+
+    for(i = 0; i < NUM_JUMP_VALUES; ++i)
+    {
+        // *INDENT-OFF*
+        vdm_jump_values[i] = (uint32_t)
+              ((int32_t)G_lppb->jump_value_set[region][i]
+            + (((int32_t)G_lppb->PsVDMJumpSlopes[region][i] * psdiff
+            // Apply the rounding adjust
+            + (int32_t)VDM_JUMP_VALUE_ADJUST) >> THRESH_SLOPE_FP_SHIFT));
+        // *INDENT-ON*
+    }
+
+    // Enforce the following:
+    // new_NL = MIN(MAX(I_NL, I_NS+1), MAX(NL[region], NL[region+1]))
+    // new_SN = MIN(I_SN, I_NS)
+    // new_LS = MIN(I_LS, new_NL - I_SN)
+    // where I_* means the calculated (interpolated) value
+    // *INDENT-OFF*
+    vdm_jump_values[VDM_N_L_IDX] = MIN(
+        MAX(vdm_jump_values[VDM_N_L_IDX], (vdm_jump_values[VDM_N_S_IDX]+1)),
+        MAX(G_lppb->jump_value_set[region][VDM_N_L_IDX],
+            G_lppb->jump_value_set[region+1][VDM_N_L_IDX]));
+    vdm_jump_values[VDM_S_N_IDX] = MIN(vdm_jump_values[VDM_S_N_IDX],
+                                       vdm_jump_values[VDM_N_S_IDX]);
+    vdm_jump_values[VDM_L_S_IDX] = MIN(vdm_jump_values[VDM_L_S_IDX],
+                                      (vdm_jump_values[VDM_N_L_IDX]
+                                     - vdm_jump_values[VDM_S_N_IDX]));
+    // *INDENT-ON*
+    // Return the jump values in bit positions as they appear in DPLL_CTRL
+    new_jump_values |= (vdm_jump_values[VDM_N_L_IDX] << SHIFT64SH(35))
+                       |  (vdm_jump_values[VDM_N_S_IDX] << SHIFT64SH(39))
+                       |  (vdm_jump_values[VDM_L_S_IDX] << SHIFT64SH(43))
+                       |  (vdm_jump_values[VDM_S_N_IDX] << SHIFT64SH(47));
+    return new_jump_values;
+}
+
+void update_vdm_jump_values_in_dpll(uint32_t pstate, uint32_t region)
+{
+    data64_t scom_data = { 0 };
+    uint32_t new_jump_values = calc_vdm_jump_values(pstate, region);
+    // Read the current contents of DPLL_CTRL and then update only the jump
+    // value fields
+    ippm_read(QPPM_DPLL_CTRL, &scom_data.value);
+
+    // This check works because the remaining bits are reserved in DPLL_CTRL[32:63]
+    if(new_jump_values != scom_data.words.lower)
+    {
+        // Critical section to ensure this entire sequence is done atomically
+        // (the nonatomic_ippm_read/write functions can be used safely)
+        PkMachineContext ctx __attribute__((unused));
+        pk_critical_section_enter(&ctx);
+
+        qppm_dpll_freq_t saved_dpll_val;
+        qppm_dpll_freq_t reduced_dpll_val;
+        // The frequency needs to be reduced by the N_L amount, this depends on
+        // if the frequency has already been changed (if raising ps, then the
+        // freq has already been dropped and the N_L value is based on that, ie. "new")
+        uint32_t adj_n_l = (pstate >= G_cme_pstate_record.quadPstate)
+                           ? (new_jump_values & BITS32(0, 4)) >> 28
+                           : (scom_data.words.lower & BITS32(0, 4)) >> 28;
+        data64_t poll_data;
+        // Read the current freq controls
+        nonatomic_ippm_read(QPPM_DPLL_FREQ, &saved_dpll_val.value);
+        // Reduce freq by N_L (in 32nds)
+        reduced_dpll_val.value = 0;
+        reduced_dpll_val.fields.fmult = (saved_dpll_val.fields.fmult
+                                         * (32 - adj_n_l)) >> 5;
+        reduced_dpll_val.fields.fmax = reduced_dpll_val.fields.fmult;
+        reduced_dpll_val.fields.fmin = reduced_dpll_val.fields.fmult;
+        // Write the reduced frequency
+        nonatomic_ippm_write(QPPM_DPLL_FREQ, reduced_dpll_val.value);
+        poll_dpll_update_complete();
+        // Clear jump enable (drop to Mode 2)
+        nonatomic_ippm_write(QPPM_DPLL_CTRL_CLR, BIT64(1));
+        // Poll for lock
+        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
+        {
+            nonatomic_ippm_read(QPPM_DPLL_STAT, &poll_data.value);
+        }
+        while(!(poll_data.words.lower & BITS32(30, 2)));
+
+        // Write the new jump values (clear jump enable)
+        scom_data.value &= ~BIT64(1);
+        scom_data.words.lower = new_jump_values;
+        nonatomic_ippm_write(QPPM_DPLL_CTRL, scom_data.value);
+        // Set jump enable (switch back to Mode 3)
+        nonatomic_ippm_write(QPPM_DPLL_CTRL_OR, BIT64(1));
+
+        // The frequency will be raised as part of the pstate transition if
+        // lowering the pstate, don't need to do anything here
+        if(pstate >= G_cme_pstate_record.quadPstate)
+        {
+            // Restore frequency
+            nonatomic_ippm_write(QPPM_DPLL_FREQ, saved_dpll_val.value);
+            poll_dpll_update_complete();
+        }
+
+        pk_critical_section_exit(&ctx);
+    }
 }
+#endif//NIMBUS_DD_LEVEL
 
 void calc_vdm_threshold_indices(uint32_t pstate, uint32_t region,
                                 uint32_t indices[])
@@ -373,11 +523,13 @@ void calc_vdm_threshold_indices(uint32_t pstate, uint32_t region,
 
     for(i = 0; i < NUM_THRESHOLD_POINTS; ++i)
     {
+        // *INDENT-OFF*
         // 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));
+            + (((int32_t)G_lppb->PsVDMThreshSlopes[region][i] * psdiff
+            // Apply the rounding adjust
+            + (int32_t)vdm_rounding_adjust[i]) >> THRESH_SLOPE_FP_SHIFT));
+        // *INDENT-ON*
     }
 
     // Check the interpolation result; since each threshold has a distinct round
@@ -449,6 +601,10 @@ void p9_cme_vdm_update(uint32_t pstate)
         ippm_write(QPPM_VDMCFGR, scom_data);
     }
     while(not_done);
+
+#if NIMBUS_DD_LEVEL != 10
+    update_vdm_jump_values_in_dpll(pstate, region);
+#endif//NIMBUS_DD_LEVEL
 }
 #endif//USE_CME_VDM_FEATURE
 
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 520aa9ca..8bea5ef8 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
@@ -92,6 +92,14 @@ typedef enum
 
 typedef enum
 {
+    VDM_N_S_IDX = 0,
+    VDM_N_L_IDX = 1,
+    VDM_L_S_IDX = 2,
+    VDM_S_N_IDX = 3
+} VDM_JUMP_VALUE_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
@@ -111,7 +119,9 @@ typedef enum
     //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_VID_COMP_ADJUST = (uint32_t)((1 << VID_SLOPE_FP_SHIFT_12) * ((float)2 / 4)),
+    //VDM_JUMP_VALUE_ADJUST
+    VDM_JUMP_VALUE_ADJUST = (uint32_t)((1 << THRESH_SLOPE_FP_SHIFT) * ((float)1 / 2))
 } VDM_ROUNDING_ADJUST;
 
 typedef enum
@@ -177,6 +187,7 @@ void p9_cme_pstate_db_handler(void*, PkIrqId);
 void p9_cme_pstate_intercme_in0_handler(void*, PkIrqId);
 void p9_cme_pstate_intercme_msg_handler(void* arg, PkIrqId irq);
 int send_pig_packet(uint64_t data, uint32_t coreMask);
+void poll_dpll_update_complete();
 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);
@@ -189,6 +200,8 @@ void p9_cme_pstate_pmsr_updt(uint32_t coreMask);
     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
+uint32_t calc_vdm_jump_values(uint32_t pstate, uint32_t region);
+void update_vdm_jump_values_in_dpll(uint32_t pstate, uint32_t region);
 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);
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 a62e3e7f..dc496ad3 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
@@ -101,7 +101,7 @@ void p9_cme_pstate_db_thread(void* arg)
     PK_TRACE_INF("DB_TH: Started\n");
     PkMachineContext  ctx __attribute__((unused));
     uint32_t cores = 0;
-    uint64_t scom_data;
+    data64_t scom_data;
     uint32_t resclk_data;
 
     G_cmeHeader = (cmeHeader_t*)(CME_SRAM_HEADER_ADDR);
@@ -159,9 +159,9 @@ void p9_cme_pstate_db_thread(void* arg)
 
         // Pstate Clocking Initialization (QM)
         // Calculate the initial pstate
-        ippm_read(QPPM_DPLL_STAT, &scom_data);
+        ippm_read(QPPM_DPLL_STAT, &scom_data.value);
         int32_t pstate = (int32_t)G_lppb->dpll_pstate0_value
-                         - (int32_t)((scom_data & BITS64(1, 11)) >> SHIFT64(11));
+                         - (int32_t)((scom_data.value & BITS64(1, 11)) >> SHIFT64(11));
         // Clip the pstate at ultra-turbo, ie. pstate=0
         G_cme_pstate_record.quadPstate = (pstate < 0) ? 0 : (uint32_t)pstate;
         PK_TRACE_INF("qm | initial pstate=%d", G_cme_pstate_record.quadPstate);
@@ -175,8 +175,8 @@ void p9_cme_pstate_db_thread(void* arg)
             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;
+            scom_data.value = (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);
@@ -185,14 +185,21 @@ void p9_cme_pstate_db_thread(void* arg)
             // 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));
+                scom_data.value |= (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 >> 32,
-                         scom_data);
-            ippm_write(QPPM_VDMCFGR, scom_data);
+            PK_TRACE_INF("qm | initial vdmcfgr=%08x%08x", scom_data.words.upper,
+                         scom_data.words.lower);
+            ippm_write(QPPM_VDMCFGR, scom_data.value);
+#if NIMBUS_DD_LEVEL != 10
+            // Slam the VDM Jump values at CME boot/init (VDMs are not enabled yet)
+            ippm_read(QPPM_DPLL_CTRL, &scom_data.value);
+            scom_data.words.lower = calc_vdm_jump_values(G_cme_pstate_record.quadPstate,
+                                    region);
+            ippm_write(QPPM_DPLL_CTRL, scom_data.value);
+#endif//NIMBUS_DD_LEVEL
             // Assumes 100us has elapsed during cache chiplet wakeup after
             // enabling the full-speed cache clock grid
             // Clear VDM Disable
@@ -250,7 +257,7 @@ void p9_cme_pstate_db_thread(void* arg)
     // Check that resonance is not enabled in CACCR and EXCGCR
     CME_GETSCOM(CPPM_CACCR, cores, scom_data);
     // Ignore clk_sync_enable, reserved, and override bits
-    resclk_data = (scom_data >> 32) & ~BITS32(13, 19);
+    resclk_data = (scom_data.value >> 32) & ~BITS32(13, 19);
 
     if(resclk_data != 0)
     {
@@ -258,11 +265,11 @@ void p9_cme_pstate_db_thread(void* arg)
     }
 
 #if NIMBUS_DD_LEVEL >= 21 || CUMULUS_DD_LEVEL > 10
-    ippm_read(QPPM_EXCGCR, &scom_data);
+    ippm_read(QPPM_EXCGCR, &scom_data.value);
     // Ignore clk_sync_enable, clkglm_async_reset, clkglm_sel, and reserved
-    scom_data &= ~(BITS64(29, 9) | BITS64(42, 22));
+    scom_data.value &= ~(BITS64(29, 9) | BITS64(42, 22));
 
-    if(scom_data != 0)
+    if(scom_data.value != 0)
     {
         PK_PANIC(CME_PSTATE_RESCLK_ENABLED_AT_BOOT);
     }
@@ -288,18 +295,18 @@ void p9_cme_pstate_db_thread(void* arg)
                 (uint32_t)G_lppb->resclk.resclk_index[0];
 
             // Extract the resclk value from QCCR
-            ippm_read(QPPM_QACCR, &scom_data);
-            scom_data = (scom_data & BITS64(0, 13)) >> SHIFT64(15);
+            ippm_read(QPPM_QACCR, &scom_data.value);
+            scom_data.value = (scom_data.value & BITS64(0, 13)) >> SHIFT64(15);
             G_cme_pstate_record.resclkData.common_resclk_idx = p9_cme_resclk_get_index(G_cme_pstate_record.quadPstate);
             // Read QACCR and clear out the resclk settings
-            ippm_read(QPPM_QACCR, &scom_data);
-            scom_data &= ~BITS64(0, 13);
+            ippm_read(QPPM_QACCR, &scom_data.value);
+            scom_data.value &= ~BITS64(0, 13);
             // OR-in the resclk settings which match the current Pstate
-            scom_data |= (((uint64_t)G_lppb->resclk.steparray
-                           [G_cme_pstate_record.resclkData.common_resclk_idx].value)
-                          << 48);
+            scom_data.value |= (((uint64_t)G_lppb->resclk.steparray
+                                 [G_cme_pstate_record.resclkData.common_resclk_idx].value)
+                                << 48);
             // Write QACCR
-            ippm_write(QPPM_QACCR, scom_data);
+            ippm_write(QPPM_QACCR, scom_data.value);
         }
 
         out32(CME_LCL_FLAGS_OR, BIT32(CME_FLAGS_RCLK_OPERABLE));
@@ -671,25 +678,46 @@ inline void p9_cme_pstate_freq_update()
 inline void p9_cme_pstate_update_analog()
 {
 #ifdef USE_CME_RESCLK_FEATURE
-    uint32_t curr = (G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
-                    ? G_cme_pstate_record.resclkData.common_resclk_idx
-                    : G_cme_pstate_record.quadPstate;
-    uint32_t next = (G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
-                    ? p9_cme_resclk_get_index(G_next_pstate)
-                    : G_next_pstate;
-#else
-    uint32_t curr = G_cme_pstate_record.quadPstate;
-    uint32_t next = G_next_pstate;
+    uint32_t rescurr = (G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+                       ? G_cme_pstate_record.resclkData.common_resclk_idx
+                       : G_cme_pstate_record.quadPstate;
+    uint32_t resnext = (G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+                       ? p9_cme_resclk_get_index(G_next_pstate)
+                       : G_next_pstate;
 #endif//USE_CME_RESCLK_FEATURE
 
-    if(next >= curr)
+#ifdef USE_CME_VDM_FEATURE
+
+    if((G_cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+       && G_next_pstate < G_cme_pstate_record.quadPstate)
     {
-        p9_cme_pstate_freq_update();
+        p9_cme_vdm_update(G_next_pstate);
     }
 
+#endif//USE_CME_VDM_FEATURE
+
 #ifdef USE_CME_RESCLK_FEATURE
 
-    if(G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+    if((G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+       && (resnext < rescurr))
+    {
+        PkMachineContext ctx;
+        pk_critical_section_enter(&ctx);
+
+        p9_cme_resclk_update(ANALOG_COMMON, G_next_pstate,
+                             G_cme_pstate_record.resclkData.common_resclk_idx);
+
+        pk_critical_section_exit(&ctx);
+    }
+
+#endif//USE_CME_RESCLK_FEATURE
+
+    p9_cme_pstate_freq_update();
+
+#ifdef USE_CME_RESCLK_FEATURE
+
+    if((G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+       && (resnext >= rescurr))
     {
         PkMachineContext ctx;
         pk_critical_section_enter(&ctx);
@@ -704,18 +732,14 @@ inline void p9_cme_pstate_update_analog()
 
 #ifdef USE_CME_VDM_FEATURE
 
-    if(G_cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+    if((G_cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+       && G_next_pstate >= G_cme_pstate_record.quadPstate)
     {
         p9_cme_vdm_update(G_next_pstate);
     }
 
 #endif//USE_CME_VDM_FEATURE
 
-    // when moving from a higher to a lower index, update resclk then freq
-    if(next < curr)
-    {
-        p9_cme_pstate_freq_update();
-    }
 }
 
 void p9_cme_pstate_update()
diff --git a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_init.c b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_init.c
index 049c3c11..a5e79976 100644
--- a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_init.c
+++ b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_init.c
@@ -456,6 +456,8 @@ p9_sgpe_stop_init()
 
     uint32_t cme_flags   = 0;
     uint16_t cmeBootList = ((~qssr.value) >> 16) & 0xFFF0;
+    sgpeHeader_t* pSgpeImgHdr = (sgpeHeader_t*)(OCC_SRAM_SGPE_HEADER_ADDR);
+
     PK_TRACE_INF("Setup: Prepare CME[%x] to be Booted", cmeBootList);
 
     for(qloop = 0; qloop < MAX_QUADS; qloop++)
@@ -501,6 +503,16 @@ p9_sgpe_stop_init()
                 scom_data.words.lower = 0;
                 scom_data.words.upper = BIT32(20) | BIT32(22) | BIT32(24);
 
+                if (pSgpeImgHdr->g_sgpe_reserve_flags & SGPE_VDM_ENABLE_BIT_POS)
+                {
+                    // If VDM function is configured to be turned on,
+                    // then CME will enable VDM at CME boot regardless if pstate is enabled,
+                    // which needs DPLL control access as early as booting time,
+                    // so done here before the boot
+                    // otherwise, when pstate is enabled, PGPE will take care of this bit.
+                    scom_data.words.upper |= BIT32(26);
+                }
+
                 // set 21, 23, 25, and 27 if EX0 is bad (first two cores in the quad are bad)
                 if (!(G_sgpe_stop_record.state[qloop].cme_flags & 0xC))
                 {