CME Code Size Reduction ATTEMPT#3

  -- some IOTA kernel cleanup
  -- also add checking for IOTA execution stack overflow
  -- re-coded to eliminate some math library macro usage
  -- added native 16-bit multiply
  -- re-coded to remove redundancy from external interrupt handler
  -- removed dec handler (optional define) and other minor cleanup
  -- fixed Interrupt initialization code in std_init (all PPE images)
  -- always inline pstate_db0_clip_bcast & update_vdm_jump_values_in_dpll
  -- optimized pls calculation code
  -- optimized pstate init, db1 handler, core good handling
  -- optimized pmcr requests and pmsr updates (always write for both cores)

Key_Cronus_Test=PM_REGRESS

Change-Id: I64f1f3a01772ab1f58c681a00c7e1a76f6ddf31a
Original-Change-Id: If48fec5832bd5e46cb89f0d6a97d90a488e8ff7b
CQ: SW415503
RTC: 178789
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/53381
Tested-by: PPE CI <ppe-ci+hostboot@us.ibm.com>
Tested-by: Hostboot CI <hostboot-ci+hostboot@us.ibm.com>
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Tested-by: Cronus HW CI <cronushw-ci+hostboot@us.ibm.com>
Tested-by: FSP CI Jenkins <fsp-CI-jenkins+hostboot@us.ibm.com>
Reviewed-by: YUE DU <daviddu@us.ibm.com>
Reviewed-by: RANGANATHPRASAD G. BRAHMASAMUDRA <prasadbgr@in.ibm.com>
Reviewed-by: Gregory S. Still <stillgs@us.ibm.com>

1 files changed, 159 insertions, 283 deletions

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 37e61b83..471c4d8f 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
@@ -55,9 +55,8 @@ extern CmePstateRecord G_cme_pstate_record;
 extern cmeHeader_t* G_cmeHeader;
 extern LocalPstateParmBlock* G_lppb;
 extern uint8_t G_vdm_threshold_table[];
-uint32_t G_cme_flags;
 cppm_cmedb0_t G_dbData;
-uint32_t G_next_pstate;
+
 //This is set and cleared during registration, so no need
 //to protect it in critical section. It only serves to make sure
 //that p9_cme_update_analog is called if pstates are started during
@@ -68,16 +67,18 @@ uint32_t G_update_analog_error;
 //
 //Function Prototypes
 //
-inline void p9_cme_pstate_register() __attribute__((always_inline));
+inline void p9_cme_pstate_register()__attribute__((always_inline));
 inline void p9_cme_pstate_db0_stop() __attribute__((always_inline));
-inline void p9_cme_pstate_db0_clip_bcast() __attribute__((always_inline));
 inline void p9_cme_pstate_db0_pmsr_updt() __attribute__((always_inline));
-inline void p9_cme_pstate_freq_update() __attribute__((always_inline));
+inline void p9_cme_pstate_freq_update(uint32_t) __attribute__((always_inline));
 inline void p9_cme_pstate_update_analog() __attribute__((always_inline));
+
+void p9_cme_pstate_init();
 void p9_cme_pstate_process_db0();
 void p9_cme_pstate_notify_sib();
 void p9_cme_pstate_db0_start();
 void p9_cme_pstate_db0_glb_bcast();
+void p9_cme_pstate_db0_clip_bcast();
 void p9_cme_pstate_update();
 void p9_cme_pstate_pig_send();
 
@@ -85,45 +86,25 @@ void p9_cme_pstate_pig_send();
 //Doorbell0 interrupt handler
 //
 //Only enabled on QuadManager-CME
-void p9_cme_pstate_db0_handler(void* arg, PkIrqId irq)
+void p9_cme_pstate_db0_handler(void)
 {
-    PK_TRACE_INF("DB_HDL: Entered\n");
-#if defined(__IOTA__)
     wrteei(1);
     p9_cme_pstate_process_db0();
-#else
-    pk_semaphore_post((PkSemaphore*)arg);
-#endif
-    PK_TRACE_INF("DB_HDL: Exited\n");
 }
 
 //
 //Doorbell3 interrupt handler
 //
 //Note: This enabled on both QuadManagerCME and SiblingCME
-void p9_cme_pstate_db3_handler(void* arg, PkIrqId irq)
+void p9_cme_pstate_db3_handler(void)
 {
     cppm_cmedb3_t    db3;
 
     PK_TRACE_INF("DB3 Handler");
 
     //Clear EISR and read DB3 register
-    if (in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_CORE0_GOOD))
-    {
-        out32(CME_LCL_EISR_CLR, BIT32(10));
-        CME_GETSCOM(CPPM_CMEDB3, CME_MASK_C0, db3.value);
-
-        //Clear DB3_C1 if CORE1 is good
-        if (in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_CORE1_GOOD))
-        {
-            out32(CME_LCL_EISR_CLR, BIT32(11));
-        }
-    }
-    else if (in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_CORE1_GOOD))
-    {
-        out32(CME_LCL_EISR_CLR, BIT32(11));
-        CME_GETSCOM(CPPM_CMEDB3, CME_MASK_C1, db3.value);
-    }
+    out32(CME_LCL_EISR_CLR, BITS32(10, 2));
+    CME_GETSCOM(CPPM_CMEDB3, G_cme_pstate_record.firstGoodCoreMask, db3.value);
 
     if(db3.fields.cme_message_numbern >= MSGID_DB3_HIGH_PRIORITY_PSTATE)
     {
@@ -144,10 +125,9 @@ void p9_cme_pstate_db3_handler(void* arg, PkIrqId irq)
 //
 void p9_cme_pstate_db3_handler_replay_db0()
 {
-    G_cme_flags = in32(CME_LCL_FLAGS);
     G_update_analog_error = 0;
 
-    PK_TRACE_INF("DB_TH: Process DB0 Replay");
+    PK_TRACE_INF("PSTATE: Process DB0 Replay");
 
     //QuadManager
     if(G_cme_pstate_record.qmFlag)
@@ -181,19 +161,19 @@ void p9_cme_pstate_db3_handler_high_priority_pstate()
 }
 
 //
-//p9_cme_db_thread
-//
-void p9_cme_pstate_db0_thread(void* arg)
+void p9_cme_pstate_init()
 {
-    PK_TRACE_INF("DB_TH: Started\n");
+    PK_TRACE_INF("PSTATE: Init Started\n");
     PkMachineContext  ctx __attribute__((unused));
     uint32_t cores = 0;
     data64_t scom_data;
+    uint64_t eimr_clr = 0;
+    uint64_t eimr_or = 0;
     uint32_t resclk_data;
 
     G_cmeHeader = (cmeHeader_t*)(CME_SRAM_HEADER_ADDR);
     G_lppb = (LocalPstateParmBlock*)(G_cmeHeader->g_cme_pstate_region_offset + CME_SRAM_BASE_ADDR);
-    PK_TRACE_INF("DB_TH: Hdr=0x%x, LPPB=0x%x, Nominal_Freq_Mhz=%d \n", (uint32_t)G_cmeHeader, (uint32_t)G_lppb,
+    PK_TRACE_INF("PSTATE: Hdr=0x%x, LPPB=0x%x, Nominal_Freq_Mhz=%d \n", (uint32_t)G_cmeHeader, (uint32_t)G_lppb,
                  G_lppb->operating_points[NOMINAL].frequency_mhz);
 
     G_register_in_progress = 0;
@@ -213,14 +193,24 @@ void p9_cme_pstate_db0_thread(void* arg)
     G_cme_record.spurr_freq_ref_upper = ((~core_freq) + 1) << 24;
 
     // Mask PMCR interrupts, these will be unmasked when starting Pstates
-    g_eimr_override |= BITS64(34, 2);
-    out32_sh(CME_LCL_EIMR_OR, (BITS64SH(34, 2)));
+    eimr_or |= BITS64(34, 2);
 
     //Read CME_LCL_FLAGS
-    G_cme_flags = in32(CME_LCL_FLAGS);
+    uint32_t cme_flags = in32(CME_LCL_FLAGS);
 
-    G_cme_pstate_record.qmFlag = G_cme_flags & BIT32(CME_FLAGS_QMGR_MASTER);
-    G_cme_pstate_record.siblingCMEFlag = G_cme_flags & BIT32(CME_FLAGS_SIBLING_FUNCTIONAL);
+    G_cme_pstate_record.qmFlag         = cme_flags & BIT32(CME_FLAGS_QMGR_MASTER);
+    G_cme_pstate_record.siblingCMEFlag = cme_flags & BIT32(CME_FLAGS_SIBLING_FUNCTIONAL);
+
+    if(cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
+    {
+        G_cme_pstate_record.firstGoodCoreMask = CME_MASK_C0;
+        cores |= CME_MASK_C0;
+    }
+    else // Core1 must be good
+    {
+        G_cme_pstate_record.firstGoodCoreMask = CME_MASK_C1;
+        cores |= CME_MASK_C1;
+    }
 
     //Enable Interrupts depending on whether this CME is
     //a quadManager or siblingCME. DB0 is enabled only
@@ -230,35 +220,25 @@ void p9_cme_pstate_db0_thread(void* arg)
     //if quadManager
     if (G_cme_pstate_record.qmFlag)
     {
-#if !defined(__IOTA__)
-        pk_semaphore_create(&G_cme_pstate_record.sem[1], 0, 1);
-#endif
-
         if(G_cme_pstate_record.siblingCMEFlag)
         {
             // Wait for the sibling to boot
             intercme_direct(INTERCME_DIRECT_IN1, INTERCME_DIRECT_NOTIFY, 1);
         }
 
-        if (G_cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
+        if (cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
         {
-            out32_sh(CME_LCL_EIMR_CLR, BIT64SH(36));//Enable DB0_0
-            out32_sh(CME_LCL_EIMR_OR, BIT64SH(37));//Disable DB0_1
-            g_eimr_override |= (BIT64(37) | BIT64(11)) ;
-            G_cme_pstate_record.cmeMaskGoodCore = CME_MASK_C0;
-            cores |= CME_MASK_C0;
+            eimr_clr |= BIT64(10) | BIT64(36); //Enable DB3_0 and DB0_0
+            eimr_or  |= BIT64(11) | BIT64(37); //Disable DB3_1 and DB0_1
         }
-        else if (G_cme_flags & BIT32(CME_FLAGS_CORE1_GOOD))
+        else
         {
-            out32_sh(CME_LCL_EIMR_OR, BIT64SH(36));//Disable DB0_0
-            out32_sh(CME_LCL_EIMR_CLR, BIT64SH(37));//Enable DB0_1
-            g_eimr_override |= (BIT64(36) | BIT64(10)) ;
-            G_cme_pstate_record.cmeMaskGoodCore = CME_MASK_C1;
-            cores |= CME_MASK_C1;
+            eimr_clr |= BIT64(11) | BIT64(37); //Enable  DB3_1 and DB0_1
+            eimr_or  |= BIT64(10) | BIT64(36); //Disable DB3_0 and DB0_0
         }
 
-        out64(CME_LCL_EIMR_OR, BIT64(7) | BIT64(38));//Disable  InterCME_IN0/IN1
-        g_eimr_override |= (BIT64(7) | BIT64(38));
+        // Disable the intercme messages used by the Sibling
+        eimr_or |= BIT64(7) | BIT64(38);
 
         // Pstate Clocking Initialization (QM)
         // Calculate the initial pstate
@@ -279,7 +259,7 @@ void p9_cme_pstate_db0_thread(void* arg)
             uint32_t region = pstate_to_vpd_region(G_cme_pstate_record.quadPstate);
             // VID compare
             scom_data.value = (uint64_t)(pstate_to_vid_compare(G_cme_pstate_record.quadPstate, region)
-                                         & BITS32(24, 8)) << 56;
+                                         & BITS32(24, 8)) << SHIFT64(7);
             // 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);
@@ -288,9 +268,9 @@ void p9_cme_pstate_db0_thread(void* arg)
             // disabled at this point.
             for(i = 0; i < NUM_THRESHOLD_POINTS; ++i)
             {
-                scom_data.value |= (uint64_t)G_vdm_threshold_table[
-                                       G_cme_pstate_record.vdmData.vdm_threshold_idx[i]]
-                                   << (52 - (i * 4));
+                scom_data.words.upper |= G_vdm_threshold_table[
+                                             G_cme_pstate_record.vdmData.vdm_threshold_idx[i]]
+                                         << (SHIFT32(11) - (i * 4));
             }
 
             PK_TRACE_INF("qm | initial vdmcfgr=%08x%08x", scom_data.words.upper,
@@ -320,20 +300,10 @@ void p9_cme_pstate_db0_thread(void* arg)
                               IMT_INIT_PSTATE);
         }
     }
-    else
+    else  // Sibling CME
     {
-        out32_sh(CME_LCL_EIMR_OR, BIT32(4) | BIT32(5));//Disable DB0_0 and DB0_1
-
-        if(G_cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
-        {
-            G_cme_pstate_record.cmeMaskGoodCore = CME_MASK_C0;
-            cores |= CME_MASK_C0;
-        }
-        else if(G_cme_flags & BIT32(CME_FLAGS_CORE1_GOOD))
-        {
-            G_cme_pstate_record.cmeMaskGoodCore = CME_MASK_C1;
-            cores |= CME_MASK_C1;
-        }
+        // Disable all DB0 and DB3 interrupts
+        eimr_or |= BITS64(36, 2) | BITS64(10, 2);
 
         // Sibling set-up for intercme messaging, after this respond to the QMs
         // notify to tell him the Sib is ready to go
@@ -341,20 +311,21 @@ void p9_cme_pstate_db0_thread(void* arg)
         out32(CME_LCL_EIPR_OR, BIT32(29));
 
         // Wait for QM to send an initial notify
-        while(!(in32_sh(CME_LCL_EISR) & BIT32(6)));
+        while(!(in32_sh(CME_LCL_EISR) & BIT64SH(38)));
 
         intercme_direct(INTERCME_DIRECT_IN1, INTERCME_DIRECT_ACK, 0);
 
         intercme_msg_recv(&G_cme_pstate_record.quadPstate, IMT_INIT_PSTATE);
         PK_TRACE_INF("sib | initial pstate=%d", G_cme_pstate_record.quadPstate);
 
-        out32(CME_LCL_EIMR_CLR, BIT32(7)); //Enable InterCME_IN0
-        g_eimr_override |= BIT64(37); //DB0_C1
-        g_eimr_override |= BIT64(36); //DB0_C0
-        g_eimr_override |= BIT64(10); //DB3_C0
-        g_eimr_override |= BIT64(11); //DB3_C1
+        eimr_clr |= BIT32(7); //Enable InterCME_IN0
     }
 
+    g_eimr_override |= eimr_or;
+    g_eimr_override &= ~eimr_clr;
+    out64(CME_LCL_EIMR_OR,  eimr_or);
+    out64(CME_LCL_EIMR_CLR, eimr_clr);
+
     // At this point, both QM and Sib can set their initial global Pstate
     G_cme_pstate_record.globalPstate = G_cme_pstate_record.quadPstate;
 
@@ -393,12 +364,12 @@ void p9_cme_pstate_db0_thread(void* arg)
 
         if(G_cme_pstate_record.qmFlag)
         {
-            G_cme_pstate_record.resclkData.l2_ex0_resclk_idx = curr_idx;
-            G_cme_pstate_record.resclkData.l2_ex1_resclk_idx = curr_idx;
-
             uint32_t next_idx = p9_cme_resclk_get_index(G_cme_pstate_record.quadPstate);
 
 #if DISABLE_STOP8 == 0
+            G_cme_pstate_record.resclkData.l2_ex0_resclk_idx = curr_idx;
+            G_cme_pstate_record.resclkData.l2_ex1_resclk_idx = curr_idx;
+
             // Directly Write QACCR without stepping, prepare for later enablement
             // since cores and L2s are currently being overridden with CACCR and EXCGCR
             G_cme_pstate_record.resclkData.common_resclk_idx = next_idx;
@@ -433,36 +404,10 @@ void p9_cme_pstate_db0_thread(void* arg)
 
 #endif//USE_CME_VDM_FEATURE
 
-    //Read CME_LCL_FLAGS again
-    G_cme_flags = in32(CME_LCL_FLAGS);
-
     //Register with PGPE
     p9_cme_pstate_register();
 
-    //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.
-    if (G_cme_pstate_record.qmFlag)
-    {
-        PK_TRACE_INF("DB_TH: Inited\n");
-
-#if !defined(__IOTA__)
-
-        while(1)
-        {
-            //pend on sempahore
-            pk_semaphore_pend(&G_cme_pstate_record.sem[1], PK_WAIT_FOREVER);
-            wrteei(1);
-
-            p9_cme_pstate_process_db0();
-
-            pk_irq_vec_restore(&ctx);
-        }
-
-#endif
-    }
-
-    PK_TRACE_INF("DB_TH: Exit\n");
+    PK_TRACE_INF("PSTATE: Init Exit\n");
 }
 
 //
@@ -471,33 +416,25 @@ void p9_cme_pstate_db0_thread(void* arg)
 void p9_cme_pstate_process_db0()
 {
     ppm_pig_t ppmPigData;
-    G_cme_flags = in32(CME_LCL_FLAGS);
     G_update_analog_error = 0;
     uint64_t scom_data   =   0;
 
-    PK_TRACE_INF("DB_TH: Process DB0 Enter\n");
+    PK_TRACE_INF("PSTATE: Process DB0 Enter\n");
 
     //Clear EISR and read DB0 register
-    //It's important that we do it in
-    if (G_cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
-    {
-        out32_sh(CME_LCL_EISR_CLR, BIT64SH(36));
-        CME_GETSCOM(CPPM_CMEDB0, CME_MASK_C0, G_dbData.value);
-    }
-    else if (G_cme_flags & BIT32(CME_FLAGS_CORE1_GOOD))
-    {
-        out32_sh(CME_LCL_EISR_CLR, BIT64SH(37));
-        CME_GETSCOM(CPPM_CMEDB0, CME_MASK_C1, G_dbData.value);
-    }
+    out32_sh(CME_LCL_EISR_CLR, BITS64SH(36, 2));
+    CME_GETSCOM(CPPM_CMEDB0, G_cme_pstate_record.firstGoodCoreMask, G_dbData.value);
 
     CME_GETSCOM_OR( CPPM_CSAR, CME_MASK_BC, scom_data );
 
-    if( ( scom_data >> 32 ) & CME_PSTATE_HCODE_ERR_INJ_BIT )
+    if( scom_data & BIT64(CPPM_CSAR_PSTATE_HCODE_ERROR_INJECT) )
     {
         PK_TRACE_ERR("CME PSTATE ERROR INJECT TRAP");
         PK_PANIC(CME_PSTATE_TRAP_INJECT);
     }
 
+    uint32_t cme_flags = in32(CME_LCL_FLAGS);
+
     //Ignore if Doorbell0 if DB0_PROCESSING_ENABLE=0. This bit is zero
     //upon CME boot. PGPE will set it right before sending Pstate Starts.
     //PGPE clears it before ACKing Stop Entry Notify(this CME is about to powered off)
@@ -507,7 +444,7 @@ void p9_cme_pstate_process_db0()
         //Process DB0
         //Start Pstates and Pstates NOT enabled
         if((G_dbData.fields.cme_message_number0 == MSGID_DB0_START_PSTATE_BROADCAST) &&
-           (!(G_cme_flags & BIT32(CME_FLAGS_PSTATES_ENABLED))))
+           (!(cme_flags & BIT32(CME_FLAGS_PSTATES_ENABLED))))
         {
             //This will Clear EISR[DB0_C0/1] to prevent another DB0 interrupt
             //PGPE multicast Global Bcast/Clip Bcast DB0, and it's possible that
@@ -525,14 +462,14 @@ void p9_cme_pstate_process_db0()
             //Otherwise, ignore. The reason is PGPE multicasts Global Bcast, and doorbell0
             //can be written while this CME is powered-off or or about to be powered-off.
             //For Pstate Start and Stop PGPE only unicasts.
-            if (G_cme_flags & BIT32(CME_FLAGS_PSTATES_ENABLED))
+            if (cme_flags & BIT32(CME_FLAGS_PSTATES_ENABLED))
             {
                 p9_cme_pstate_db0_glb_bcast();
             }
         }
         //Stop Pstates and Pstates enabled
         else if((G_dbData.fields.cme_message_number0 == MSGID_DB0_STOP_PSTATE_BROADCAST) &&
-                (G_cme_flags & BIT32(CME_FLAGS_PSTATES_ENABLED)))
+                (cme_flags & BIT32(CME_FLAGS_PSTATES_ENABLED)))
         {
             p9_cme_pstate_db0_stop();
         }
@@ -551,8 +488,8 @@ void p9_cme_pstate_process_db0()
             ppmPigData.value = 0;
             ppmPigData.fields.req_intr_type = 4;
             ppmPigData.fields.req_intr_payload = MSGID_PCB_TYPE4_ACK_ERROR;
-            send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
-            PK_TRACE_INF("DB_TH: Bad DB0=0x%x Pstate=0x%x", G_dbData.fields.cme_message_number0, G_cme_flags);
+            send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
+            PK_TRACE_INF("PSTATE: Bad DB0=0x%x", (uint8_t)G_dbData.fields.cme_message_number0);
 
             if(G_dbData.fields.cme_message_number0 < MSGID_DB0_VALID_START ||
                G_dbData.fields.cme_message_number0 > MSGID_DB0_VALID_END)
@@ -566,40 +503,34 @@ void p9_cme_pstate_process_db0()
         }
     }
 
-    PK_TRACE_INF("DB_TH: Process DB0 Exit\n");
+    PK_TRACE_INF("PSTATE: Process DB0 Exit\n");
 }
 
 
 inline void p9_cme_pstate_register()
 {
     uint32_t done = 0;
+    uint32_t cme_flags = in32(CME_LCL_FLAGS);
     G_register_in_progress = 1;
 
     //Quad Manager sends register message to PGPE
     if (G_cme_pstate_record.qmFlag)
     {
         //Clear EISR[DB0_C0/1]
-        if (G_cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
-        {
-            out32_sh(CME_LCL_EISR_CLR, BIT64SH(36));
-        }
-        else
-        {
-            out32_sh(CME_LCL_EISR_CLR, BIT64SH(37));
-        }
+        out32_sh(CME_LCL_EISR_CLR, BITS64SH(36, 2));
 
         //Send type4(ack doorbell)
         ppm_pig_t ppmPigData;
         ppmPigData.value = 0;
         ppmPigData.fields.req_intr_type = 4;
         ppmPigData.fields.req_intr_payload =  MSGID_PCB_TYPE4_QUAD_MGR_AVAILABLE;
-        send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
+        send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
 
-        PK_TRACE_INF("DB_TH: Register Msg Sent\n");
+        PK_TRACE_INF("PSTATE: Register Msg Sent\n");
 
-        if (G_cme_flags & BIT32(CME_FLAGS_WAIT_ON_PSTATE_START))      //Poll on DB0
+        if (cme_flags & BIT32(CME_FLAGS_WAIT_ON_PSTATE_START))      //Poll on DB0
         {
-            PK_TRACE_INF("DB_TH: Wait on Pstate Start\n");
+            PK_TRACE_INF("PSTATE: Wait on Pstate Start\n");
 
             //Wait until PGPE has set DB0_PROCESSING_ENABLE. This is important as
             //PGPE might send DB0 to other quads as part of processing registration
@@ -607,25 +538,21 @@ inline void p9_cme_pstate_register()
             //PGPE has set DB0_PROCESSING_ENABLE
             while(!(in32(CME_LCL_SRTCH0) & BIT32(CME_SCRATCH_DB0_PROCESSING_ENABLE)));
 
+            PK_TRACE_INF("PSTATE: DB0 Processing is Enabled\n");
+
             //PGPE will send Pmin, Pmax and Pstate Start Doorbells
             //Pstate Start is the last DB0
             while(!done)
             {
-                //Wait until DB0 is written again
-                if (G_cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
-                {
-                    while(!(in32_sh(CME_LCL_EISR) & BIT64SH(36))) {}
+                uint32_t db0_check = G_cme_pstate_record.firstGoodCoreMask << SHIFT64SH(37);
 
-                    out32_sh(CME_LCL_EISR_CLR, BIT64SH(36));
-                    CME_GETSCOM(CPPM_CMEDB0, CME_MASK_C0, G_dbData.value);
-                }
-                else
-                {
-                    while(!(in32_sh(CME_LCL_EISR) & BIT64SH(37))) {}
+                //Wait until DB0 is written again (looking only at the first good CPPM)
 
-                    out32_sh(CME_LCL_EISR_CLR, BIT64SH(37));
-                    CME_GETSCOM(CPPM_CMEDB0, CME_MASK_C1, G_dbData.value);
-                }
+                while(!(in32_sh(CME_LCL_EISR) & db0_check)) {}
+
+                out32_sh(CME_LCL_EISR_CLR, db0_check);
+
+                CME_GETSCOM(CPPM_CMEDB0, G_cme_pstate_record.firstGoodCoreMask, G_dbData.value);
 
                 //Process DB0
                 if((G_dbData.fields.cme_message_number0 == MSGID_DB0_START_PSTATE_BROADCAST))
@@ -646,22 +573,24 @@ inline void p9_cme_pstate_register()
     {
         uint32_t msgCnt = 0;
 
-        if (G_cme_flags & BIT32(CME_FLAGS_WAIT_ON_PSTATE_START))
+        if (cme_flags & BIT32(CME_FLAGS_WAIT_ON_PSTATE_START))
         {
             //PGPE sends Pmin, Pmax, and Pstate start DB0, so
             //total of three
-            PK_TRACE_INF("DB_TH: Wait on Pstate Start\n");
+            PK_TRACE_INF("PSTATE: Wait on Pstate Start\n");
 
             while(msgCnt != 3)
             {
                 PK_TRACE_INF("DB_TH: Sib Register MsgCnt=0x%xt", msgCnt);
                 p9_cme_pstate_sibling_lock_and_intercme_protocol(1);
                 msgCnt++;
+                PK_TRACE_INF("PSTATE: Sib Register MsgCnt=%d\n", msgCnt);
             }
         }
     }
 
     G_register_in_progress = 0;
+    PK_TRACE_INF("PSTATE: Register Done\n");
 }
 
 //
@@ -669,21 +598,13 @@ inline void p9_cme_pstate_register()
 //
 void p9_cme_pstate_db0_start()
 {
-    PK_TRACE_INF("DB_TH: DB0 Start Enter\n");
+    PK_TRACE_INF("PSTATE: DB0 Start Enter\n");
     ppm_pig_t ppmPigData;
 
     p9_cme_pstate_update();
 
-
     //Clear EISR[DB0_C0/1] to prevent another interrupt
-    if (G_cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
-    {
-        out32_sh(CME_LCL_EISR_CLR, BIT32(4));
-    }
-    else
-    {
-        out32_sh(CME_LCL_EISR_CLR, BIT32(5));
-    }
+    out32_sh(CME_LCL_EISR_CLR, BITS64SH(36, 2));
 
     //Prepare PPM type4 payload for ACK/NACK for PGPE
     //Send NACK, if any errors. Otherwise, send ACK
@@ -700,29 +621,28 @@ void p9_cme_pstate_db0_start()
 
         out32(CME_LCL_FLAGS_OR, BIT32(24));//Set Pstates Enabled
 
-        //Clear Pending PMCR interrupts and Enable PMCR Interrupts (for good cores)
-        g_eimr_override |= BITS64(34, 2);
-        g_eimr_override &= ~(uint64_t)(G_cme_record.core_enabled << 28);
+        //Enable PMCR Interrupts (for good cores) when this task is done
+        g_eimr_override &= ~(uint64_t)(G_cme_record.core_enabled << SHIFT64(35));
 
         //Clear Core GPMMR RESET_STATE_INDICATOR bit to show pstates have started
         CME_PUTSCOM(PPM_GPMMR_CLR, G_cme_record.core_enabled, BIT64(15));
     }
 
     //Send type4(ack doorbell)
-    send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
+    send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
 
     //Reset G_update_analog_error
     G_update_analog_error = 0;
 
-    PK_TRACE_INF("DB_TH: DB0 Start Exit\n");
+    PK_TRACE_INF("PSTATE: DB0 Start Exit\n");
 }
 
 //
 //Doorbell0 Global Broadcast
 //
-void p9_cme_pstate_db0_glb_bcast(cppm_cmedb0_t dbData)
+void p9_cme_pstate_db0_glb_bcast()
 {
-    PK_TRACE_INF("DB_TH: DB0 GlbBcast Enter\n");
+    PK_TRACE_INF("PSTATE: DB0 GlbBcast Enter\n");
     ppm_pig_t ppmPigData;
 
     p9_cme_pstate_update();
@@ -742,9 +662,9 @@ void p9_cme_pstate_db0_glb_bcast(cppm_cmedb0_t dbData)
     }
 
     //Send type4(ack doorbell)
-    send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
+    send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
     G_update_analog_error = 0;
-    PK_TRACE_INF("DB_TH: DB0 GlbBcast Exit\n");
+    PK_TRACE_INF("PSTATE: DB0 GlbBcast Exit\n");
 }
 
 //
@@ -752,7 +672,7 @@ void p9_cme_pstate_db0_glb_bcast(cppm_cmedb0_t dbData)
 //
 inline void p9_cme_pstate_db0_stop()
 {
-    PK_TRACE_INF("DB_TH: DB0 Stop Enter\n");
+    PK_TRACE_INF("PSTATE: DB0 Stop Enter\n");
     ppm_pig_t ppmPigData;
 
     out32(CME_LCL_FLAGS_CLR, BIT32(24));//Set Pstates Disabled
@@ -763,27 +683,30 @@ inline void p9_cme_pstate_db0_stop()
 
     //PGPE will update the LMCR[0] before sending the STOP PSTATE Doorbell.
     //Here we update the PMSR to indicate that Pstates are no longer honored accordingly.
-    p9_cme_pstate_pmsr_updt(G_cme_record.core_enabled);
+    p9_cme_pstate_pmsr_updt();
     p9_cme_pstate_notify_sib(); //Notify sibling
 
     //Send type4(ack doorbell)
     ppmPigData.value = 0;
     ppmPigData.fields.req_intr_type = 4;
     ppmPigData.fields.req_intr_payload = MSGID_PCB_TYPE4_ACK_PSTATE_SUSPENDED;
-    send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
+    send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
 
     //Set Core GPMMR RESET_STATE_INDICATOR bit to show pstates have stopped
     CME_PUTSCOM(PPM_GPMMR_OR, G_cme_record.core_enabled, BIT64(15));
 
-    PK_TRACE_INF("DB_TH: DB0 Stop Exit\n");
+    PK_TRACE_INF("PSTATE: DB0 Stop Exit\n");
 }
 
-inline void p9_cme_pstate_db0_clip_bcast()
+void p9_cme_pstate_db0_clip_bcast()
 {
 
-    PK_TRACE_INF("DB_TH: DB0 Clip Enter\n");
+    PK_TRACE_INF("PSTATE: DB0 Clip Enter\n");
+
     ppm_pig_t ppmPigData;
+
     uint32_t dbBit8_15 = (G_dbData.value & BITS64(8, 8)) >> SHIFT64(15);
+
     uint32_t dbQuadValue = (G_dbData.value >> (in32(CME_LCL_SRTCH0) &
                             (BITS32(CME_SCRATCH_LOCAL_PSTATE_IDX_START,
                                     CME_SCRATCH_LOCAL_PSTATE_IDX_LENGTH)))) & 0xFF;
@@ -797,7 +720,7 @@ inline void p9_cme_pstate_db0_clip_bcast()
         G_cme_pstate_record.pmax = dbQuadValue;
     }
 
-    PK_TRACE_INF("DB_TH: Pmin=0x%x,Pmax=0x%x", G_cme_pstate_record.pmin, G_cme_pstate_record.pmax);
+    PK_TRACE_INF("PSTATE: Pmin=0x%x,Pmax=0x%x", G_cme_pstate_record.pmin, G_cme_pstate_record.pmax);
 
     if(G_cme_pstate_record.siblingCMEFlag)
     {
@@ -805,21 +728,21 @@ inline void p9_cme_pstate_db0_clip_bcast()
         intercme_msg_send(0, IMT_LOCK_SIBLING);
     }
 
-    p9_cme_pstate_pmsr_updt(G_cme_record.core_enabled);
+    p9_cme_pstate_pmsr_updt();
     p9_cme_pstate_notify_sib(); //Notify sibling
 
     //Send type4(ack doorbell)
     ppmPigData.value = 0;
     ppmPigData.fields.req_intr_type = 4;
     ppmPigData.fields.req_intr_payload = MSGID_PCB_TYPE4_ACK_PSTATE_PROTO_ACK;
-    send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
+    send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
 
-    PK_TRACE_INF("DB_TH: DB0 Clip Exit\n");
+    PK_TRACE_INF("PSTATE: DB0 Clip Exit\n");
 }
 
 inline void p9_cme_pstate_db0_pmsr_updt()
 {
-    PK_TRACE_INF("DB_TH: DB0 Pmsr Updt Enter\n");
+    PK_TRACE_INF("PSTATE: DB0 Pmsr Updt Enter\n");
     ppm_pig_t ppmPigData;
 
     uint32_t dbBit8_15 = (G_dbData.value & BITS64(8, 8)) >> SHIFT64(15);
@@ -839,7 +762,7 @@ inline void p9_cme_pstate_db0_pmsr_updt()
             break;
     }
 
-    p9_cme_pstate_pmsr_updt(G_cme_record.core_enabled);
+    p9_cme_pstate_pmsr_updt();
 
     p9_cme_pstate_notify_sib(); //Notify sibling
 
@@ -847,17 +770,17 @@ inline void p9_cme_pstate_db0_pmsr_updt()
     ppmPigData.value = 0;
     ppmPigData.fields.req_intr_type = 4;
     ppmPigData.fields.req_intr_payload = MSGID_PCB_TYPE4_ACK_PSTATE_SUSPENDED;
-    send_pig_packet(ppmPigData.value, G_cme_pstate_record.cmeMaskGoodCore);
+    send_pig_packet(ppmPigData.value, G_cme_pstate_record.firstGoodCoreMask);
 
     //Set Core GPMMR RESET_STATE_INDICATOR bit to show pstates have stopped
     CME_PUTSCOM(PPM_GPMMR_OR, G_cme_record.core_enabled, BIT64(15));
 
-    PK_TRACE_INF("DB_TH: DB0 Safe Mode Exit\n");
+    PK_TRACE_INF("PSTATE: DB0 Safe Mode Exit\n");
 }
 
 void p9_cme_pstate_notify_sib()
 {
-    PK_TRACE_INF("DB_TH: Notify Enter\n");
+    PK_TRACE_INF("PSTATE: Notify Enter\n");
 
     //Notify sibling CME(if any)
     if(G_cme_pstate_record.siblingCMEFlag)
@@ -874,27 +797,25 @@ void p9_cme_pstate_notify_sib()
 
 }
 
-//
-//p9_cme_pstate_freq_update
-//
-inline void p9_cme_pstate_freq_update()
+inline void p9_cme_pstate_freq_update(uint32_t cme_flags)
 {
-    if(G_cme_flags & BIT32(CME_FLAGS_FREQ_UPDT_DISABLE))
+    if(cme_flags & BIT32(CME_FLAGS_FREQ_UPDT_DISABLE))
     {
-        PK_TRACE_INF("DB_TH: Freq Updt Disabled\n");
+        PK_TRACE_INF("PSTATE: Freq Updt Disabled\n");
     }
     else
     {
-        PK_TRACE_INF("DB_TH: Freq Updt Enter\n");
-        PK_TRACE_INF("DB_TH: Dpll0=0x%x\n", G_lppb->dpll_pstate0_value);
-        PK_TRACE_INF("DB_TH: Hdr=0x%x, LPPB=0x%x\n", (uint32_t)G_cmeHeader, (uint32_t)G_lppb);
+        PK_TRACE_INF("PSTATE: Freq Updt Enter\n");
+        PK_TRACE_INF("PSTATE: Dpll0=0x%x\n", G_lppb->dpll_pstate0_value);
+        PK_TRACE_INF("PSTATE: Hdr=0x%x, LPPB=0x%x\n", (uint32_t)G_cmeHeader, (uint32_t)G_lppb);
+
         //Adjust DPLL
         qppm_dpll_freq_t dpllFreq;
 
         //Write new value of DPLL using INTERPPM
         //Currently, fmax, fmin and fmult are all written with the same value
         dpllFreq.value = 0;
-        dpllFreq.fields.fmax  = G_lppb->dpll_pstate0_value - G_next_pstate;
+        dpllFreq.fields.fmax  = G_lppb->dpll_pstate0_value - G_cme_pstate_record.nextPstate;
         dpllFreq.fields.fmult = dpllFreq.fields.fmax;
         dpllFreq.fields.fmin  = dpllFreq.fields.fmax;
 
@@ -906,73 +827,26 @@ inline void p9_cme_pstate_freq_update()
 
         ippm_write(QPPM_DPLL_FREQ, dpllFreq.value);
 
-        /*
-        // DPLL Modes
-        //                     enable_fmin    enable_fmax   enable_jump
-        // DPLL Mode  2             0              0             0
-        // DPLL Mode  3             0              0             1
-        // DPLL Mode  4             X              1             0
-        // DPLL Mode  4             1              X             0
-        // DPLL Mode  3.5           0              1             1
-        // DPLL Mode  5             1              X             1
-        // TODO Future Attributes
-        // DROOP_PROTECT          -> DPLL Mode 3
-        // DROOP_PROTECT_OVERVOLT -> DPLL Mode 3.5
-        // DYNAMIC                -> DPLL Mode 4
-        // DYNAMIC_PROTECT        -> DPLL Mode 5
-
-        uint32_t cme_flags = in32(CME_LCL_FLAGS);
-        data64_t scom_data = { 0 };
-
-        // DPLL Mode 2
-        if(!(cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE)))
-        {
-            PK_TRACE_INF("Poll on DPLL_STAT[freq_change=0]");
-
-            // ... 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 & BIT32(29)));
-        }
-
-        // DPLL Mode 3
-        if(cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
-        {
-
-            PK_TRACE_INF("Poll on DPLL_STAT[update_complete=1]");
-            // ... 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)));
-        }*/
         G_update_analog_error = poll_dpll_stat();
 
-        PK_TRACE_INF("DB_TH: Freq Updt Exit\n");
+        PK_TRACE_INF("PSTATE: Freq Updt Exit\n");
     }
 }
 
 inline void p9_cme_pstate_update_analog()
 {
-    // Get the current state of CME flags
-    G_cme_flags = in32(CME_LCL_FLAGS);
-
     do
     {
+
+        uint32_t cme_flags = in32(CME_LCL_FLAGS);
+
 #ifdef USE_CME_VDM_FEATURE
 
         // if increasing voltage (decreasing Pstate) then change VDM threshold settings before changing frequency
-        if((G_cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
-           && G_next_pstate < G_cme_pstate_record.quadPstate)
+        if((cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+           && G_cme_pstate_record.nextPstate < G_cme_pstate_record.quadPstate)
         {
-            G_update_analog_error = p9_cme_vdm_update(G_next_pstate);
+            G_update_analog_error = p9_cme_vdm_update(G_cme_pstate_record.nextPstate);
         }
 
         if (G_update_analog_error)
@@ -983,18 +857,15 @@ inline void p9_cme_pstate_update_analog()
 #endif //USE_CME_VDM_FEATURE
 
 #ifdef USE_CME_RESCLK_FEATURE
-        // protect entire IF BLOCK in a critical section
-        // in case a stop11 entry happens during Pstate change
 
-        uint32_t rescurr = (G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+        uint32_t rescurr = (cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
                            ? G_cme_pstate_record.resclkData.common_resclk_idx
                            : 0;
 
-        uint32_t resnext = (G_cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
-                           ? p9_cme_resclk_get_index(G_next_pstate)
+        uint32_t resnext = (cme_flags & BIT32(CME_FLAGS_RCLK_OPERABLE))
+                           ? p9_cme_resclk_get_index(G_cme_pstate_record.nextPstate)
                            : 0;
 
-
         if(resnext < rescurr)
         {
             p9_cme_resclk_update(ANALOG_COMMON, resnext, rescurr);
@@ -1002,12 +873,13 @@ inline void p9_cme_pstate_update_analog()
 
 #endif//USE_CME_RESCLK_FEATURE
 
-        p9_cme_pstate_freq_update();
+        p9_cme_pstate_freq_update(cme_flags);
 
+        //If error changing DPLL freq, skip the code below
         if (G_update_analog_error)
         {
             break;
-        } //If error skip the code below
+        }
 
 #ifdef USE_CME_RESCLK_FEATURE
 
@@ -1021,10 +893,15 @@ inline void p9_cme_pstate_update_analog()
 #ifdef USE_CME_VDM_FEATURE
 
         // if decreasing voltage (increasing Pstate) then change VDM threshold settings after changing frequency
-        if((G_cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
-           && G_next_pstate >= G_cme_pstate_record.quadPstate)
+        if((cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+           && G_cme_pstate_record.nextPstate >= G_cme_pstate_record.quadPstate)
         {
-            G_update_analog_error = p9_cme_vdm_update(G_next_pstate);
+            G_update_analog_error = p9_cme_vdm_update(G_cme_pstate_record.nextPstate);
+
+            if (G_update_analog_error)
+            {
+                break;
+            }
         }
 
 #endif//USE_CME_VDM_FEATURE
@@ -1037,41 +914,40 @@ void p9_cme_pstate_update()
 {
     PkMachineContext ctx;
 
-    PK_TRACE_INF("DB_TH: Pstate Updt Enter");
+    PK_TRACE_INF("PSTATE: Pstate Updt Enter");
 
-    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.nextPstate = (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);
 
-    if(G_next_pstate != G_cme_pstate_record.quadPstate ||
+    if((G_cme_pstate_record.nextPstate != G_cme_pstate_record.quadPstate) ||
        G_register_in_progress)
     {
         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()
+            // The Sibling is a "pumpkin" from this point forward until
+            // we call p9_cme_pstate_notify_sib()
         }
 
-
-        PK_TRACE_INF("DB_TH: DBData=0x%08x%08x\n", G_dbData.value >> 32,
+        PK_TRACE_INF("PSTATE: DBData=0x%08x%08x\n", G_dbData.value >> 32,
                      G_dbData.value);
 
-        p9_cme_pstate_pmsr_updt_in_progress(G_cme_record.core_enabled);
+        p9_cme_pstate_pmsr_updt_in_progress();
 
         pk_critical_section_enter(&ctx);
 
         p9_cme_pstate_update_analog();
 
-        //Update quadPstate only no error
+        //Update quadPstate only if no error
         if(!G_update_analog_error)
         {
             // Must update quadPstate before calling PMSR update
-            G_cme_pstate_record.quadPstate = G_next_pstate;
+            G_cme_pstate_record.quadPstate = G_cme_pstate_record.nextPstate;
         }
         else
         {
@@ -1081,9 +957,9 @@ void p9_cme_pstate_update()
         pk_critical_section_exit(&ctx);
     }
 
-    p9_cme_pstate_pmsr_updt(G_cme_record.core_enabled);
+    p9_cme_pstate_pmsr_updt();
 
-    p9_cme_pstate_notify_sib(); // Notify Sibling
+    p9_cme_pstate_notify_sib();
 
-    PK_TRACE_INF("DB_TH: Pstate Updt Exit");
+    PK_TRACE_INF("PSTATE: Pstate Updt Exit");
 }