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: 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>

7 files changed, 614 insertions, 650 deletions

diff --git a/import/chips/p9/procedures/ppe_closed/cme/iota_lnk_cfg.h b/import/chips/p9/procedures/ppe_closed/cme/iota_lnk_cfg.h
index 824bdca1..430a935c 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/iota_lnk_cfg.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/iota_lnk_cfg.h
@@ -37,8 +37,8 @@
 #define PPE_DEBUG_PTRS_OFFSET    CME_DEBUG_PTRS_OFFSET
 #define PPE_DEBUG_PTRS_SIZE      CME_DEBUG_PTRS_SIZE
 
-#define PPE_DUMP_PTR_PSTATE_SIZE 0x48
+#define PPE_DUMP_PTR_PSTATE_SIZE 0x4C
 #define PPE_DUMP_PTR_STOP_SIZE   0x28
-#define PPE_DUMP_PTR_COMMON_SIZE 0x4
+#define PPE_DUMP_PTR_COMMON_SIZE 0x8
 
 #endif
diff --git a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_iota_main.c b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_iota_main.c
index ccd65138..c36ee54e 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_iota_main.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_iota_main.c
@@ -31,16 +31,8 @@
 // CME Pstate Header and Structure
 #include "p9_cme.h"
 
-#if !DISABLE_PERIODIC_CORE_QUIESCE && (NIMBUS_DD_LEVEL == 20 || NIMBUS_DD_LEVEL == 21 || CUMULUS_DD_LEVEL == 10)
-
-CmeRecord G_cme_record = {0, 0, {0, 0, 0, 0, 0xFFFFFFFF, 0}};
-
-#else
-
 CmeRecord G_cme_record = {0, 0};
 
-#endif
-
 // CME Pstate Header and Structure
 #include "p9_cme_pstate.h"
 CmePstateRecord G_cme_pstate_record __attribute__((section (".dump_ptr_pstate")));
@@ -49,454 +41,100 @@ CmePstateRecord G_cme_pstate_record __attribute__((section (".dump_ptr_pstate"))
 #include "p9_cme_stop.h"
 CmeStopRecord G_cme_stop_record __attribute__((section (".dump_ptr_stop"))) = {{0}, {0}, 0, 0, 0, 0, 0, 0, 0, {0}};
 
-// NDD2: OOB bits wired to SISR
-//       not implemented in DD1
-// bit0 is System checkstop
-// bit1 is Recoverable Error
-// bit2 is Special Attention
-// bit3 is Core Checkstop
-#define bad_error_present (((in32   (CME_LCL_SISR) & (  BIT32(12) |   BITS32(14,2)))) || \
-                           ((in32_sh(CME_LCL_SISR) & (BIT64SH(60) | BITS64SH(62,2)))))
-
-
 #if !DISABLE_PERIODIC_CORE_QUIESCE && (NIMBUS_DD_LEVEL == 20 || NIMBUS_DD_LEVEL == 21 || CUMULUS_DD_LEVEL == 10)
-
-inline static
-void periodic_core_quiesce_workaround(uint32_t core_instruction_running)
-{
-    uint32_t core;
-    uint32_t core_accessible;
-    uint32_t fused_core_mode;
-    uint32_t spattn_offset;
-    uint32_t spattn[2];
-    uint32_t maint_mode[2];
-    uint32_t time_stamp[2];
-    data64_t scom_data;
-    uint32_t sample_error = 0;
-    uint32_t saved_msr = 0;
-
-    PK_TRACE("FIT: Periodic Core Quiesce Workaround");
-
-    CME_GETSCOM_AND(CPPM_CPMMR, CME_MASK_BC, scom_data.value);
-    fused_core_mode = scom_data.words.upper & BIT32(9);
-
-    PK_TRACE("PCQW: Fused Core Mode[%x]", fused_core_mode);
-
-    //0) in case in stop0/1 that we dont know about
-
-    PK_TRACE("PCQW: Assert block interrupt to PC via SICR[2/3]");
-    out32(CME_LCL_SICR_OR, core_instruction_running << SHIFT32(3));
-
-    PK_TRACE("PCQW: Waking up the core(pm_exit=1) via SICR[4/5]");
-    out32(CME_LCL_SICR_OR, core_instruction_running << SHIFT32(5));
-
-    CME_PM_EXIT_DELAY
-
-    PK_TRACE("PCQW: Polling for core wakeup(pm_active=0) via EINR[20/21]");
-
-    while((in32(CME_LCL_EINR)) & (core_instruction_running << SHIFT32(21)));
-
-    //1) Acquire Pcb Mux
-
-    core_accessible = ((~in32(CME_LCL_SISR)) >> SHIFT32(11)) & core_instruction_running;
-
-    PK_TRACE("PCQW: Request PCB Mux via SICR[10/11]");
-    out32(CME_LCL_SICR_OR, core_accessible << SHIFT32(11));
-
-    // Poll Infinitely for PCB Mux Grant
-    while((core_accessible & (in32(CME_LCL_SISR) >> SHIFT32(11))) != core_accessible);
-
-    PK_TRACE("PCQW: PCB Mux Granted");
-
-    //2) Read RAS_STATUS Scom Addr(20:31) = x0A02
-    //   bit (0 + 8*T) where (T= thread) CORE_MAINT_MODE
-    //   to find out which threads are in maintenance mode
-
-    // vector = 0 + 8*T as a shifting base used below
-#define THREAD_VECTOR (BIT32(0)|BIT32(8)|BIT32(16)|BIT32(24))
-
-    for(core = CME_MASK_C0; core > 0; core--)
-    {
-        if (core & core_instruction_running)
-        {
-            CME_GETSCOM(RAS_STATUS, core, scom_data.value) ;
-            maint_mode[core & 1] = scom_data.words.upper & THREAD_VECTOR;
-        }
-    }
-
-
-    //3) Write DIRECT_CONTROLS Scom Addr(20:31) = x0A9C
-    //   bit (7 + 8*T) where (T= thread) DC_CORE_STOP for ALL threads.
-    //   This will quiesce the active threads,
-    //   put all threads into core maintenance mode,
-    //   and eventually quiesce the entire core.
-
-    scom_data.words.lower = 0;
-    scom_data.words.upper = THREAD_VECTOR >> 7;
-
-#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
-
-    for(core = CME_MASK_C0; core > 0; core--)
-    {
-        if (core & core_instruction_running)
-        {
-#else
-
-    core = core_instruction_running;
-
-#endif
-
-            // The SCOM can be delayed by traffic on PC on the SPR bus, so it is possible
-            // to get a RC=4 (Address Error), which really indicates a timeout.   Need to mask
-            // this return code and retry until we get a clean return code
-            saved_msr = mfmsr();
-            mtmsr( saved_msr | MSR_SEM4);  // Mask off timeout
-
-            do
-            {
-                CME_PUTSCOM_NOP(DIRECT_CONTROLS, core, scom_data.value);
-            }
-            while ((mfmsr() & MSR_SIBRC) != 0);
-
-            mtmsr(saved_msr);
-
-#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
-        }
-    }
-
-#endif
-
-
-    //4) Loop on RAS_STATUS Scom Addr(20:31) = x0A02
-    //   until bit(1 + 8*T) THREAD_QUIESCE are all active b1
-
-    time_stamp[0] = in32(CME_LCL_TBR);
-
-#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
-
-    for(core = CME_MASK_C0; core > 0; core--)
-    {
-        if (core & core_instruction_running)
-        {
-#else
-
-    core = core_instruction_running;
-
-#endif
-
-#define THREAD_VECTOR_CHECK (THREAD_VECTOR>>1 | THREAD_VECTOR>>3)
-
-// In a future release of this patch, it should be based on the Nest Frequency, but
-// plumbing for that sill needs to be created.
-// 200us in 32ns timer ticks
-#define QUIESCE_ABORT_TICKS 0x186A
-
-            // Poll on THREAD_QUIESCE, LSU_QUIESCE, and NEST_ACTIVE.
-            // If they do not quiesce in 200us abort the patch and restart the cores.
-
-            do
-            {
-                CME_GETSCOM_AND(RAS_STATUS, core, scom_data.value);
-
-                time_stamp[1] = in32(CME_LCL_TBR);
-
-                if (time_stamp[1] > time_stamp[0])
-                {
-                    G_cme_record.fit_record.core_quiesce_time_latest =
-                        time_stamp[1] - time_stamp[0];
-                }
-                else
-                {
-                    G_cme_record.fit_record.core_quiesce_time_latest =
-                        0xFFFFFFFF - time_stamp[0] + time_stamp[1] + 1;
-                }
-            }
-            while((((scom_data.words.upper& THREAD_VECTOR_CHECK) != THREAD_VECTOR_CHECK)
-                   ||    //THREAD_ and LSU_QUIESCE must be ones
-                   ((scom_data.words.lower& BIT64SH(32))))  // NEST_ACTIVE must be zero
-                  && !(sample_error = bad_error_present)
-                  && (G_cme_record.fit_record.core_quiesce_time_latest < QUIESCE_ABORT_TICKS)    // 200us in 32ns timer ticks
-                 );
-
-#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
-        }
-    }
-
-#endif
-
-    if (!sample_error && (G_cme_record.fit_record.core_quiesce_time_latest < QUIESCE_ABORT_TICKS) )
-    {
-        PK_TRACE("FIT: Both Cores Quiesced");
-    }
-    else
-    {
-        PK_TRACE_INF("FIT: Error while trying to Quiesce Cores.  Bad Error %d, QuiesceTime (ns) %d", sample_error,
-                     (G_cme_record.fit_record.core_quiesce_time_latest << 5));
-        G_cme_record.fit_record.core_quiesce_failed_count++;
-    }
-
-
-    for(core = CME_MASK_C0; core > 0; core--)
-    {
-        if (core & core_instruction_running)
-        {
-            //5) Read SPATTN Scom Addr(20:31) = x0A98 to check for ATTN
-            //   (need to do this after all threads quiesce to close windows)
-
-            CME_GETSCOM(SPATTN_READ, core, scom_data.value);
-
-            if (fused_core_mode)
-            {
-                //Fused Core Mode
-                // C0 vtid0=ltid0 bit1  -> tv.bit0
-                // C0 vtid1=ltid2 bit9  -> tv.bit8
-                // C0 vtid2=ltid4 bit17 -> tv.bit16
-                // C0 vtid3=ltid6 bit25 -> tv.bit24
-                //
-                // C1 vtid0=ltid1 bit5  -> tv.bit0
-                // C1 vtid1=ltid3 bit13 -> tv.bit8
-                // C1 vtid2=ltid5 bit21 -> tv.bit16
-                // C1 vtid3=ltid7 bit29 -> tv.bit24
-                spattn_offset = ((core & 1) << 2) + 1; // C0:1, C1:5
-                spattn[core & 1] = ((scom_data.words.upper & BIT32((0 + spattn_offset))) << spattn_offset) | //0
-                                   ((scom_data.words.upper & BIT32((8 + spattn_offset))) << spattn_offset) | //8
-                                   ((scom_data.words.upper & BIT32((16 + spattn_offset))) << spattn_offset) | //16
-                                   ((scom_data.words.upper & BIT32((24 + spattn_offset))) << spattn_offset); //24
-            }
-            else
-            {
-                // Normal Mode
-                // vtid0=ltid0 bit1  -> tv.bit0
-                // vtid1=ltid1 bit5  -> tv.bit8
-                // vtid2=ltid2 bit9  -> tv.bit16
-                // vtid3=ltid3 bit13 -> tv.bit24
-                spattn[core & 1] = ((scom_data.words.upper & BIT32(1))  << 1 ) | //0
-                                   ((scom_data.words.upper & BIT32(5))  >> 3 ) | //8
-                                   ((scom_data.words.upper & BIT32(9))  >> 7 ) | //16
-                                   ((scom_data.words.upper & BIT32(13)) >> 11);  //24
-            }
-
-
-            //6) Write DIRECT_CONTROLS Scom Addr(20:31) = x0A9C
-            //   bit (3 + 8*T) where (T= thread) DC_CLEAR_MAINT for all threads
-            //   which were not in maintenance mode in step 1 AND do not have ATTN set in step 4
-
-            scom_data.words.lower = 0;
-            scom_data.words.upper =
-                (THREAD_VECTOR & (~maint_mode[core & 1]) & (~spattn[core & 1])) >> 3;
-            CME_PUTSCOM_NOP(DIRECT_CONTROLS, core, scom_data.value);
-        }
-    }
-
-    PK_TRACE("FIT: Both Cores Started");
-
-    //7) Drop pm_exit
-
-    PK_TRACE("PCQW: Drop pm_exit via SICR[4/5]");
-    out32(CME_LCL_SICR_CLR, core_instruction_running << SHIFT32(5));
-
-    PK_TRACE("PCQW: Drop block interrupt to PC via SICR[2/3]");
-    out32(CME_LCL_SICR_CLR, core_instruction_running << SHIFT32(3));
-
-    //8) Release Pcb Mux on Both Cores
-
-    PK_TRACE("PCQW: Release PCB Mux back on Both Cores via SICR[10/11]");
-    out32(CME_LCL_SICR_CLR, core_accessible << SHIFT32(11));
-
-    while((core_accessible & ~(in32(CME_LCL_SISR) >> SHIFT32(11))) != core_accessible);
-
-    PK_TRACE("PCQW: PCB Mux Released on Both Cores");
-
-
-    G_cme_record.fit_record.core_quiesce_total_count += 1;
-
-    //Profile time
-
-    // timestamp delta was computed above to handle the abort case
-
-    if (G_cme_record.fit_record.core_quiesce_time_latest <
-        G_cme_record.fit_record.core_quiesce_time_min)
-    {
-        G_cme_record.fit_record.core_quiesce_time_min =
-            G_cme_record.fit_record.core_quiesce_time_latest;
-    }
-    else if (G_cme_record.fit_record.core_quiesce_time_latest >
-             G_cme_record.fit_record.core_quiesce_time_max)
-    {
-        G_cme_record.fit_record.core_quiesce_time_max =
-            G_cme_record.fit_record.core_quiesce_time_latest;
-    }
-}
-
+CmeFitRecord G_cme_fit_record = {0, 0, 0, 0, 0xFFFFFFFF, 0};
 #endif
 
+#if !DISABLE_CME_FIT_TIMER
 
 void fit_handler()
 {
 
-#if !DISABLE_CME_FIT_TIMER
-
     mtspr(SPRN_TSR, TSR_FIS);
     PK_TRACE("FIT Timer Handler");
 
-#endif
-
 #if !DISABLE_PERIODIC_CORE_QUIESCE && (NIMBUS_DD_LEVEL == 20 || NIMBUS_DD_LEVEL == 21 || CUMULUS_DD_LEVEL == 10)
-
-    uint32_t core_quiesce_cpmmr_disable;
-    uint32_t core;
-    uint32_t core_instr_running;
-    uint32_t scom_op;
-    data64_t scom_data;
-
-#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
-
-    scom_op = CME_SCOM_NOP;
-
-    for(core = CME_MASK_C0; core > 0; core--)
-    {
-
-#else
-
-    scom_op = CME_SCOM_OR;
-    core    = CME_MASK_BC;
-
-#endif
-
-        CME_GETSCOM_OP(CPPM_CPMMR, core, scom_op, scom_data.value);
-        core_quiesce_cpmmr_disable = scom_data.words.upper & BIT32(2);
-
-#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
-
-    }
-
-#endif
-
-    if (!core_quiesce_cpmmr_disable)
-    {
-        out32(CME_LCL_FLAGS_OR,  BIT32(CME_FLAGS_CORE_QUIESCE_ACTIVE));
-    }
-    else
-    {
-        out32(CME_LCL_FLAGS_CLR, BIT32(CME_FLAGS_CORE_QUIESCE_ACTIVE));
-    }
-
-    // only run workaround if
-    // 1) both cores are enabled
-    // 2) at least one core is running
-    //    (stop entry clears the counter)
-    // 3) both cores doesnt have special_wakeup_done asserted
-    //    (spwu_done clears the counter)
-    // 4) both core doesnt have cpmmr[2] asserted
-    // 5) no bad error occurs
-
-    // Get instruction running per core
-    core_instr_running = (in32_sh(CME_LCL_SISR) >> SHIFT64SH(47))& CME_MASK_BC;
-
-    if((G_cme_record.core_enabled == CME_MASK_BC) &&
-       (core_instr_running != 0) &&
-       (!(in32(CME_LCL_SISR) & BITS32(16, 2))) &&
-       (!core_quiesce_cpmmr_disable) &&
-       (!bad_error_present))
-    {
-        if (G_cme_record.fit_record.core_quiesce_fit_trigger < 10)
-        {
-            G_cme_record.fit_record.core_quiesce_fit_trigger++;
-        }
-        else
-        {
-            G_cme_record.fit_record.core_quiesce_fit_trigger = 0;
-            periodic_core_quiesce_workaround(core_instr_running);
-        }
-    }
-
+    p9_cme_core_livelock_buster();
 #endif
 
 }
+#endif //fit handler
 
-
-
+#if ENABLE_CME_DEC_TIMER
 void dec_handler()
 {
+    mtspr(SPRN_TSR, TSR_DIS);
+    PK_TRACE("DEC Timer Handler");
 }
+#endif
 
 
-
-void ext_handler(uint32_t task_idx)
+void p9_cme_hipri_ext_handler(uint32_t task_idx)
 {
-    uint32_t eisr = in32(CME_LCL_EISR);
+    //Only look at bits 0,1,2,3, and 5
+    uint32_t eisr_subset = in32(CME_LCL_EISR) & BITS32(0, 6);
+    //exclude bit 4: PGPE Heartbeat lost
+    eisr_subset &= ~BIT32(4);
 
-    if (eisr & BIT32(0))
-    {
-        PK_TRACE_INF("CME DEBUGGER DETECTED");
-        PK_OPTIONAL_DEBUG_HALT(CME_DEBUGGER_DETECTED);
-        out32(CME_LCL_EISR_CLR, BIT32(0));
-    }
+    PK_TRACE_ERR("CME HIGHEST PRIORITY EXCEPTION DETECTED. EISR(0:7) = %02x", eisr_subset >> 24 );
 
-    if (eisr & BIT32(1))
-    {
-        PK_TRACE_INF("CME DEBUG TRIGGER DETECTED");
-        PK_OPTIONAL_DEBUG_HALT(CME_DEBUG_TRIGGER_DETECTED);
-        out32(CME_LCL_EISR_CLR, BIT32(1));
-    }
 
-    if (eisr & BIT32(2))
-    {
-        PK_TRACE_INF("CME_QUAD_CHECKSTOP DETECTED");
-        PK_OPTIONAL_DEBUG_HALT(CME_QUAD_CHECKSTOP_DETECTED);
-        out32(CME_LCL_EISR_CLR, BIT32(2));
-    }
+    // report and clear only the first one found
+    // multiples of this priority will cause repeated interrupts until they are all cleared
+    uint32_t bitnum = cntlz32(eisr_subset);
 
-    if (eisr & BIT32(3))
-    {
-        PK_TRACE_INF("CME_PVREF_FAIL DETECTED");
-        PK_OPTIONAL_DEBUG_HALT(CME_PVREF_FAIL_DETECTED);
-        out32(CME_LCL_EISR_CLR, BIT32(3));
-    }
+    if(in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_PM_DEBUG_HALT_ENABLE))
+        switch (bitnum)
+        {
+            case 0:
+                PK_PANIC(CME_DEBUGGER_DETECTED);
 
-    if (eisr & BIT32(4))
-    {
-        PK_TRACE_INF("CME OCC HEARTBEAT LOST");
-        PK_OPTIONAL_DEBUG_HALT(CME_OCC_HEARTBEAT_LOST_DETECTED);
-        out32(CME_LCL_EISR_CLR, BIT32(4));
-    }
+            case 1:
+                PK_PANIC(CME_DEBUG_TRIGGER_DETECTED);
 
-    if (eisr & BIT32(5))
-    {
-        PK_TRACE_INF("CME_CORE_CHECKSTOP DETECTED");
-        PK_OPTIONAL_DEBUG_HALT(CME_CORE_CHECKSTOP_DETECTED);
-        out32(CME_LCL_EISR_CLR, BIT32(5));
-    }
-}
+            case 2:
+                PK_PANIC(CME_QUAD_CHECKSTOP_DETECTED);
 
+            case 3:
+                PK_PANIC(CME_PVREF_FAIL_DETECTED);
 
-// List of low priority tasks that run when the cme engine would
-// otheriwse be idle.
-IOTA_BEGIN_IDLE_TASK_TABLE
-{ IOTA_IDLE_DISABLED, IOTA_NO_TASK },
-IOTA_END_IDLE_TASK_TABLE
+            case 5:
+                PK_PANIC(CME_CORE_CHECKSTOP_DETECTED);
+
+            default:
+                break;
+        }
+
+    uint32_t eisr_mask = (1 << (31 - bitnum));
+    out32(CME_LCL_EISR_CLR, eisr_mask);
+}
 
 IOTA_BEGIN_TASK_TABLE
-IOTA_TASK(ext_handler), // bits 0-6   default
-          IOTA_TASK(p9_cme_pstate_db3_handler), // bits 10,11 default
-          IOTA_TASK(p9_cme_stop_db2_handler), // bits 18,19 p9_cme_stop_db2_handler
-          IOTA_TASK(p9_cme_stop_spwu_handler), // bits 14,15 p9_cme_stop_spwu_handler
-          IOTA_TASK(p9_cme_stop_rgwu_handler), // bits 16,17 p9_cme_stop_rgwu_handler
-          IOTA_TASK(p9_cme_stop_pcwu_handler), // bits 12,13 p9_cme_stop_pcwu_handler
-          IOTA_TASK(p9_cme_stop_enter_handler), // bits 20,21 p9_cme_stop_enter_handler
-          IOTA_TASK(p9_cme_stop_db1_handler), // bits 40,41 p9_cme_stop_db1_handler
-          IOTA_TASK(p9_cme_pstate_db0_handler), // bits 36,37 p9_cme_pstate_db0_handler
-          IOTA_TASK(p9_cme_pstate_intercme_in0_irq_handler), // bit  7 p9_cme_pstate_intercme_in0_handler
-          IOTA_TASK(p9_cme_pstate_pmcr_handler), // bits 34,35 p9_cme_pstate_pmcr_handler
-          IOTA_TASK(p9_cme_pstate_intercme_msg_handler), // bit  29    p9_cme_pstate_intercme_msg_handler
+IOTA_TASK(p9_cme_hipri_ext_handler),  // bits 0-3,5
+          IOTA_TASK(p9_cme_pstate_db3_handler), // bits 10,11
+          IOTA_TASK(p9_cme_stop_db2_handler),   // bits 18,19
+          IOTA_TASK(p9_cme_stop_spwu_handler),  // bits 14,15
+          IOTA_TASK(p9_cme_stop_rgwu_handler),  // bits 16,17
+          IOTA_TASK(p9_cme_stop_pcwu_handler),  // bits 12,13
+          IOTA_TASK(p9_cme_stop_enter_handler), // bits 20,21
+          IOTA_TASK(p9_cme_stop_db1_handler),   // bits 40,41
+          IOTA_TASK(p9_cme_pstate_db0_handler), // bits 36,37
+          IOTA_TASK(p9_cme_pstate_intercme_in0_irq_handler), // bit  7
+          IOTA_TASK(p9_cme_pstate_pmcr_handler), // bits 34,35
+          IOTA_TASK(p9_cme_pstate_intercme_msg_handler), // bit  29
           IOTA_NO_TASK  // Should never see these
           IOTA_END_TASK_TABLE;
 
 int main()
 {
     // Register Timer Handlers
+#if ENABLE_CME_DEC_TIMER
     IOTA_DEC_HANDLER(dec_handler);
+#endif
+
+#if !DISABLE_CME_FIT_TIMER
     IOTA_FIT_HANDLER(fit_handler);
+#endif
 
     // Local timebase frequency comes from an attribute.
     cmeHeader_t* cmeHeader = (cmeHeader_t*)(CME_SRAM_HEADER_ADDR);
@@ -527,21 +165,32 @@ int main()
     PK_TRACE("Set Watch Dog Timer Rate to 6 and FIT Timer Rate to 8");
     out32(CME_LCL_TSEL, (BITS32(1, 2) | BIT32(4)));
 
-#if !DISABLE_CME_FIT_TIMER
+#if (!DISABLE_CME_FIT_TIMER || ENABLE_CME_DEC_TIMER)
+
+    uint32_t TCR_VAL = 0;
 
+#if !DISABLE_CME_FIT_TIMER
     PK_TRACE("Enable FIT Timer");
-    mtspr(SPRN_TCR, (TCR_FIE));
+    TCR_VAL |= TCR_FIE;
+#endif
+
+#if ENABLE_CME_DEC_TIMER
+    PK_TRACE("Enable DEC Timer");
+    TCR_VAL |= TCR_DIE;
+#endif
+
+    mtspr(SPRN_TCR, TCR_VAL);
 
 #endif
 
+    // Initialize the Stop state and Pstate tasks
     p9_cme_stop_init();
+    p9_cme_pstate_init();
 
-    // In IOTA, these have become initialization routines, not threads
-    p9_cme_stop_exit_thread(NULL);
-    p9_cme_stop_enter_thread(NULL);
-    p9_cme_pstate_db0_thread(NULL);
-    p9_cme_pstate_pmcr_thread(NULL);
+    //sync CME pair after all inits are done and indicate to SGPE
+    p9_cme_init_done();
 
+    // start IOTA and never return!
     iota_run();
 
     return 0;
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 b87635f3..4c9d3210 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
@@ -60,6 +60,8 @@ LocalPstateParmBlock* G_lppb;
 extern CmePstateRecord G_cme_pstate_record;
 extern CmeRecord G_cme_record;
 
+inline uint32_t update_vdm_jump_values_in_dpll(uint32_t pstate, uint32_t region) __attribute__((always_inline));
+
 const uint8_t G_vdm_threshold_table[13] =
 {
     0x00, 0x01, 0x03, 0x02, 0x06, 0x07, 0x05, 0x04,
@@ -91,11 +93,10 @@ int send_pig_packet(uint64_t data, uint32_t coreMask)
 uint32_t poll_dpll_stat()
 {
     data64_t data;
-    uint32_t cme_flags = in32(CME_LCL_FLAGS);
     uint32_t rc = 0;
 
     // DPLL Mode 2
-    if(!(cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE)))
+    if(!(in32(CME_LCL_FLAGS) & BIT32(CME_FLAGS_VDM_OPERABLE)))
     {
         PK_TRACE_INF("Poll on DPLL_STAT[freq_change=0]");
 
@@ -107,9 +108,8 @@ uint32_t poll_dpll_stat()
         }
         while((data.words.lower & BIT64SH(61)));
     }
-
-    // DPLL Mode 3
-    if(cme_flags & BIT32(CME_FLAGS_VDM_OPERABLE))
+    else
+        // DPLL Mode 3
     {
         PK_TRACE_INF("Poll on DPLL_STAT[update_complete=1]");
         // ... to indicate that the DPLL has sampled the newly requested
@@ -120,14 +120,7 @@ uint32_t poll_dpll_stat()
         csar.value = 0;
 
         //Read CPPM_CSAR
-        if (cme_flags & BIT32(CME_FLAGS_CORE0_GOOD))
-        {
-            CME_GETSCOM(CPPM_CSAR, CME_MASK_C0, csar.value);
-        }
-        else if (cme_flags & BIT32(CME_FLAGS_CORE1_GOOD))
-        {
-            CME_GETSCOM(CPPM_CSAR, CME_MASK_C1, csar.value);
-        }
+        CME_GETSCOM(CPPM_CSAR, G_cme_pstate_record.firstGoodCoreMask, csar.value);
 
         //Read TimebaseStart
         tbStart = in32(CME_LCL_TBR);
@@ -175,19 +168,19 @@ uint32_t poll_dpll_stat()
 // 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!
+    // G_cme_pstate_record.firstGoodCoreMask MUST be set!
     uint64_t val;
 
     cppm_ippmcmd_t cppm_ippmcmd;
     cppm_ippmcmd.value = 0;
     cppm_ippmcmd.fields.qppm_reg = addr & 0x000000ff;
     cppm_ippmcmd.fields.qppm_rnw = 1;
-    CME_PUTSCOM(CPPM_IPPMCMD, G_cme_pstate_record.cmeMaskGoodCore,
+    CME_PUTSCOM(CPPM_IPPMCMD, G_cme_pstate_record.firstGoodCoreMask,
                 cppm_ippmcmd.value);
 
     do
     {
-        CME_GETSCOM(CPPM_IPPMSTAT, G_cme_pstate_record.cmeMaskGoodCore, val);
+        CME_GETSCOM(CPPM_IPPMSTAT, G_cme_pstate_record.firstGoodCoreMask, val);
     } // Check the QPPM_ONGOING bit
 
     while(val & BIT64(0));
@@ -198,7 +191,7 @@ void nonatomic_ippm_read(uint32_t addr, uint64_t* data)
         PK_PANIC(CME_PSTATE_IPPM_ACCESS_FAILED);
     }
 
-    CME_GETSCOM(CPPM_IPPMRDATA, G_cme_pstate_record.cmeMaskGoodCore, val);
+    CME_GETSCOM(CPPM_IPPMRDATA, G_cme_pstate_record.firstGoodCoreMask, val);
 
     *data = val;
 }
@@ -215,21 +208,21 @@ void ippm_read(uint32_t addr, uint64_t* data)
 // 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!
+    // G_cme_pstate_record.firstGoodCoreMask MUST be set!
     uint64_t val;
 
-    CME_PUTSCOM(CPPM_IPPMWDATA, G_cme_pstate_record.cmeMaskGoodCore,
+    CME_PUTSCOM(CPPM_IPPMWDATA, G_cme_pstate_record.firstGoodCoreMask,
                 data);
     cppm_ippmcmd_t cppm_ippmcmd;
     cppm_ippmcmd.value = 0;
     cppm_ippmcmd.fields.qppm_reg = addr & 0x000000ff;
     cppm_ippmcmd.fields.qppm_rnw = 0;
-    CME_PUTSCOM(CPPM_IPPMCMD, G_cme_pstate_record.cmeMaskGoodCore,
+    CME_PUTSCOM(CPPM_IPPMCMD, G_cme_pstate_record.firstGoodCoreMask,
                 cppm_ippmcmd.value);
 
     do
     {
-        CME_GETSCOM(CPPM_IPPMSTAT, G_cme_pstate_record.cmeMaskGoodCore, val);
+        CME_GETSCOM(CPPM_IPPMSTAT, G_cme_pstate_record.firstGoodCoreMask, val);
     } // Check the QPPM_ONGOING bit
 
     while(val & BIT64(0));
@@ -454,10 +447,35 @@ uint32_t pstate_to_vpd_region(uint32_t pstate)
 
 uint32_t pstate_to_vid_compare(uint32_t pstate, uint32_t region)
 {
+    // prevent compiler from using software multiply
+    // to index the array of structs
+    uint32_t base_pstate;
+    compile_assert(NumSlopeRegions, VPD_NUM_SLOPES_REGION == 3);
+
+    if (region == 0)
+    {
+        base_pstate = G_lppb->operating_points[0].pstate;
+    }
+    else if (region == 1)
+    {
+        base_pstate = G_lppb->operating_points[1].pstate;
+    }
+    else
+    {
+        base_pstate = G_lppb->operating_points[2].pstate;
+    }
+
+    uint32_t  psdiff = base_pstate - pstate;
+// uint32_t  psdiff = (uint32_t)G_lppb->operating_points[region].pstate - pstate;
+
     // *INDENT-OFF*
-    return((((uint32_t)G_lppb->PsVIDCompSlopes[region]
-             * ((uint32_t)G_lppb->operating_points[region].pstate - pstate)
+        // use native PPE 16-bit multiply instruction
+        // VID codes must by definition have positive slope so use unsigned
+        // delta = slope times difference in pstate (interpolate)
+    return(((mulu16((uint32_t)G_lppb->PsVIDCompSlopes[region], psdiff)
+            // Apply the rounding adjust
              + VDM_VID_COMP_ADJUST) >> VID_SLOPE_FP_SHIFT_12)
+            // Offset value at bottom of the range
              + (uint32_t)G_lppb->vid_point_set[region]);
     // *INDENT-ON*
 }
@@ -466,16 +484,41 @@ uint32_t pstate_to_vid_compare(uint32_t pstate, uint32_t region)
 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;
+    uint32_t i;
 
-    for(i = 0; i < NUM_JUMP_VALUES; ++i)
+    // prevent compiler from using software multiply
+    // to index the array of structs
+    uint32_t base_pstate;
+    compile_assert(NumSlopeRegions, VPD_NUM_SLOPES_REGION == 3);
+
+    if (region == 0)
+    {
+        base_pstate = G_lppb->operating_points[0].pstate;
+    }
+    else if (region == 1)
+    {
+        base_pstate = G_lppb->operating_points[1].pstate;
+    }
+    else
+    {
+        base_pstate = G_lppb->operating_points[2].pstate;
+    }
+
+    int32_t  psdiff = base_pstate - pstate;
+// int32_t  psdiff = (uint32_t)G_lppb->operating_points[region].pstate - pstate;
+
+    for(i = 0; i < NUM_JUMP_VALUES; ++i )
     {
         // *INDENT-OFF*
+        // use native PPE 16-bit multiply instruction
+        // jump values can decrease with voltage so must use signed
+        // Cast every math term into 32b for more efficient PPE maths
         vdm_jump_values[i] = (uint32_t)
+            // Offset by value at bottom of the range
               ((int32_t)G_lppb->jump_value_set[region][i]
-            + (((int32_t)G_lppb->PsVDMJumpSlopes[region][i] * psdiff
+            // delta = slope times difference in pstate (interpolate)
+            + ((muls16((int32_t)G_lppb->PsVDMJumpSlopes[region][i] , psdiff)
             // Apply the rounding adjust
             + (int32_t)VDM_JUMP_VALUE_ADJUST) >> THRESH_SLOPE_FP_SHIFT));
         // *INDENT-ON*
@@ -505,7 +548,7 @@ uint32_t calc_vdm_jump_values(uint32_t pstate, uint32_t region)
     return new_jump_values;
 }
 
-uint32_t update_vdm_jump_values_in_dpll(uint32_t pstate, uint32_t region)
+inline uint32_t update_vdm_jump_values_in_dpll(uint32_t pstate, uint32_t region)
 {
     uint32_t rc = 0;
     data64_t scom_data = { 0 };
@@ -535,8 +578,8 @@ uint32_t update_vdm_jump_values_in_dpll(uint32_t pstate, uint32_t region)
         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.fmult = mulu16(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
@@ -593,15 +636,38 @@ void calc_vdm_threshold_indices(uint32_t pstate, uint32_t region,
         VDM_XTREME_ADJUST
     };
 
-    uint32_t i = 0;
-    int32_t psdiff = (uint32_t)G_lppb->operating_points[region].pstate - pstate;
+    // prevent compiler from using software multiply
+    // to index the array of structs
+    uint32_t base_pstate;
+    compile_assert(NumSlopeRegions, VPD_NUM_SLOPES_REGION == 3);
+
+    if (region == 0)
+    {
+        base_pstate = G_lppb->operating_points[0].pstate;
+    }
+    else if (region == 1)
+    {
+        base_pstate = G_lppb->operating_points[1].pstate;
+    }
+    else
+    {
+        base_pstate = G_lppb->operating_points[2].pstate;
+    }
+
+    uint32_t  psdiff = base_pstate - pstate;
+// int32_t psdiff = (uint32_t)G_lppb->operating_points[region].pstate - pstate;
+
+    uint32_t i;
 
     for(i = 0; i < NUM_THRESHOLD_POINTS; ++i)
     {
         // *INDENT-OFF*
+        // use native PPE 16-bit multiply instruction
+        // jump values can decrease with voltage so must use signed
         // 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
+            // delta = slope times difference in pstate (interpolate)
+            + ((muls16((int32_t)G_lppb->PsVDMThreshSlopes[region][i] , psdiff)
             // Apply the rounding adjust
             + (int32_t)vdm_rounding_adjust[i]) >> THRESH_SLOPE_FP_SHIFT));
         // *INDENT-ON*
@@ -628,19 +694,20 @@ uint32_t p9_cme_vdm_update(uint32_t pstate)
 {
     // Static forces this array into .sbss instead of calling memset()
     static uint32_t new_idx[NUM_THRESHOLD_POINTS] = { 0 };
-    uint32_t i = 0, rc = 0;
+    uint32_t i, rc = 0;
     // Set one bit per threshold starting at bit 31 (28,29,30,31)
     uint32_t not_done = BITS32(32 - NUM_THRESHOLD_POINTS, NUM_THRESHOLD_POINTS);
-    uint64_t scom_data = 0;
-    uint64_t base_scom_data = 0;
+    data64_t scom_data;
+    data64_t base_scom_data;
     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;
+    // Populate the VID compare field and init all other bits to zero
+    base_scom_data.value = (uint64_t)(pstate_to_vid_compare(pstate, region)
+                                      & BITS32(24, 8)) << SHIFT64(7);
 
     // Step all thresholds in parallel until each reaches its new target.
     // Doing this in parallel minimizes the number of interppm scom writes.
@@ -668,12 +735,12 @@ uint32_t p9_cme_vdm_update(uint32_t pstate)
             }
 
             // 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));
+            scom_data.words.upper |= G_vdm_threshold_table[
+                                         G_cme_pstate_record.vdmData.vdm_threshold_idx[i]]
+                                     << (SHIFT32(11) - (i * 4));
         }
 
-        ippm_write(QPPM_VDMCFGR, scom_data);
+        ippm_write(QPPM_VDMCFGR, scom_data.value);
     }
     while(not_done);
 
@@ -757,62 +824,50 @@ void p9_cme_resclk_update(ANALOG_TARGET target, uint32_t next_idx, uint32_t curr
 }
 #endif//USE_CME_RESCLK_FEATURE
 
-void p9_cme_pstate_pmsr_updt(uint32_t coreMask)
+// always update both cores regardless of partial good or stop state
+//
+void p9_cme_pstate_pmsr_updt()
 {
-    uint32_t cm;
     uint64_t pmsrData;
 
-    //CORE0 mask is 0x2, and CORE1 mask is 0x1.
-    //We AND with 0x1 to get the core number from mask.
-    for (cm = 2; cm > 0; cm--)
-    {
-        if (cm & coreMask)
-        {
-            //Note: PMSR[58/UPDATE_IN_PROGRESS] is always cleared here
-            pmsrData = ((uint64_t)G_cme_pstate_record.globalPstate) << 56;
-            pmsrData |= (((uint64_t)(G_cme_pstate_record.quadPstate)) << 48) ;
-            pmsrData |= (((uint64_t)(G_cme_pstate_record.pmin)) << 40) ;
-            pmsrData |= (((uint64_t)(G_cme_pstate_record.pmax)) << 32) ;
-
-            //LMCR[0] = 1 means PMCR SCOM update are enabled ie.
-            //PMCR SPR does not control Pstates. We reflect that in
-            //PMSR[32/PMCR_DISABLED]
-            if ((in32(CME_LCL_LMCR) & BIT32(0)))
-            {
-                pmsrData |= BIT64(32);
-            }
+    //Note: PMSR[58/UPDATE_IN_PROGRESS] is always cleared here
+    pmsrData  = ((uint64_t)G_cme_pstate_record.globalPstate) << 56;
+    pmsrData |= ((uint64_t)(G_cme_pstate_record.quadPstate)) << 48;
+    pmsrData |= ((uint64_t)(G_cme_pstate_record.pmin)) << 40;
+    pmsrData |= ((uint64_t)(G_cme_pstate_record.pmax)) << 32;
 
-            //PMSR[33] is SAFE_MODE bit
-            if(G_cme_pstate_record.safeMode)
-            {
-                pmsrData |= BIT64(33);
-            }
+    //LMCR[0] = 1 means PMCR SCOM update are enabled ie.
+    //PMCR SPR does not control Pstates. We reflect that in
+    //PMSR[32/PMCR_DISABLED]
+    if ((in32(CME_LCL_LMCR) & BIT32(0)))
+    {
+        pmsrData |= BIT64(32);
+    }
 
-            //PMSR[35] is PSTATES_SUSPENDED bit
-            if(G_cme_pstate_record.pstatesSuspended)
-            {
-                pmsrData |= BIT64(35);
-            }
+    //PMSR[33] is SAFE_MODE bit
+    if(G_cme_pstate_record.safeMode)
+    {
+        pmsrData |= BIT64(33);
+    }
 
-            out64(CME_LCL_PMSRS0 + ((cm & 0x1) << 5), pmsrData);
-        }
+    //PMSR[35] is PSTATES_SUSPENDED bit
+    if(G_cme_pstate_record.pstatesSuspended)
+    {
+        pmsrData |= BIT64(35);
     }
+
+    out64(CME_LCL_PMSRS0, pmsrData);
+    out64(CME_LCL_PMSRS1, pmsrData);
 }
 
 
-void p9_cme_pstate_pmsr_updt_in_progress(uint32_t coreMask)
+// this is needed for our test exercisers to know
+// when to check that DVFS changes are complete
+// always update both cores regardless of partial good or stop state
+//
+void p9_cme_pstate_pmsr_updt_in_progress()
 {
-    uint32_t cm;
-    uint64_t pmsrData;
 
-    //CORE0 mask is 0x2, and CORE1 mask is 0x1.
-    //We AND with 0x1 to get the core number from mask.
-    for (cm = 2; cm > 0; cm--)
-    {
-        if (cm & coreMask)
-        {
-            pmsrData = in64(CME_LCL_PMSRS0 + ((cm & 0x1) << 5)) | BIT64(PMSR_UPDATE_IN_PROGRESS);
-            out64(CME_LCL_PMSRS0 + ((cm & 0x1) << 5), pmsrData);
-        }
-    }
+    out64(CME_LCL_PMSRS0, in64(CME_LCL_PMSRS0) | BIT64(PMSR_UPDATE_IN_PROGRESS));
+    out64(CME_LCL_PMSRS1, in64(CME_LCL_PMSRS1) | BIT64(PMSR_UPDATE_IN_PROGRESS));
 }
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop.h b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop.h
index 36b615ed..7cafec64 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop.h
@@ -185,12 +185,16 @@ enum CME_IRQ_VECTORS
 #endif
     IRQ_VEC_SGPE_C0                  = BIT64(12) | BIT64(20),
     IRQ_VEC_SGPE_C1                  = BIT64(13) | BIT64(21),
+    IRQ_VEC_PCWU_C0_UPPER            = BIT32(12),
+    IRQ_VEC_PCWU_C1_UPPER            = BIT32(13),
     IRQ_VEC_PCWU_C0                  = BIT64(12),
     IRQ_VEC_PCWU_C1                  = BIT64(13),
     IRQ_VEC_SPWU_C0                  = BIT64(14),
     IRQ_VEC_SPWU_C1                  = BIT64(15),
     IRQ_VEC_RGWU_C0                  = BIT64(16),
     IRQ_VEC_RGWU_C1                  = BIT64(17),
+    IRQ_VEC_STOP_C0_UPPER            = BIT32(20),
+    IRQ_VEC_STOP_C1_UPPER            = BIT32(21),
     IRQ_VEC_STOP_C0                  = BIT64(20),
     IRQ_VEC_STOP_C1                  = BIT64(21)
 };
@@ -276,9 +280,6 @@ typedef struct
     // store panic code indicating where and what that certain core encountered error
     // mostly from various xstop detection or failed clock operation through stages of code
     uint32_t      error_code[2];
-#if !defined(__IOTA__)
-    PkSemaphore   sem[2];
-#endif
 } CmeStopRecord;
 
 
@@ -292,21 +293,20 @@ void p9_cme_stop_init();
 void p9_cme_stop_eval_eimr_override();
 void p9_cme_stop_core_error_handler(uint32_t, uint32_t, uint32_t);
 
-void p9_cme_stop_enter_thread(void*);
-void p9_cme_stop_exit_thread(void*);
+void p9_cme_core_livelock_buster();
 
 void p9_cme_stop_entry();
 void p9_cme_stop_exit();
 
 // CME STOP Interrupt Handlers
-void p9_cme_stop_enter_handler(void*, PkIrqId);
+void p9_cme_stop_enter_handler(void);
 
-void p9_cme_stop_pcwu_handler(void*, PkIrqId);
-void p9_cme_stop_rgwu_handler(void*, PkIrqId);
-void p9_cme_stop_spwu_handler(void*, PkIrqId);
+void p9_cme_stop_pcwu_handler(void);
+void p9_cme_stop_rgwu_handler(void);
+void p9_cme_stop_spwu_handler(void);
 
-void p9_cme_stop_db1_handler(void*, PkIrqId);
-void p9_cme_stop_db2_handler(void*, PkIrqId);
+void p9_cme_stop_db1_handler(void);
+void p9_cme_stop_db2_handler(void);
 
 // CME STOP Utility Functions
 void p9_hcd_core_scan0(uint32_t, uint64_t, uint64_t);
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_entry.c b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_entry.c
index 559bcf9b..009be29f 100755
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_entry.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_entry.c
@@ -705,6 +705,7 @@ p9_cme_stop_entry()
                 PK_TRACE("RAMMING Set SPR mode to LT0-7 via SPR_MODE[20-27]");
                 CME_PUTSCOM(SPR_MODE, core, BITS64(20, 8));
 
+
                 if (core & CME_MASK_C0)
                 {
                     PK_TRACE("RAMMING Set SPRC to scratch0 for core0 via SCOM_SPRC");
@@ -718,46 +719,39 @@ p9_cme_stop_entry()
                     CME_PUTSCOM(SCOM_SPRC, CME_MASK_C1, BIT64(60));
                     CME_GETSCOM(SCRATCH1, CME_MASK_C1, G_scratch[1]);
                 }
-            }
 
-            if ((core & CME_MASK_C0) && G_cme_stop_record.req_level[0] > STOP_LEVEL_3)
-            {
+                uint32_t pls_core = ((G_cme_stop_record.req_level[0] > STOP_LEVEL_3) ? (core & CME_MASK_C0) : 0)
+                                    | ((G_cme_stop_record.req_level[1] > STOP_LEVEL_3) ? (core & CME_MASK_C1) : 0);
+
                 for(thread = 0; thread < 4; thread++)
                 {
                     PK_TRACE("PSSCR RAM: mfspr psscr, gpr0 via RAM_CTRL");
-                    CME_PUTSCOM(RAM_CTRL, CME_MASK_C0, RAM_MFSPR_PSSCR_GPR0 | (((uint64_t) thread) << 62));
+                    CME_PUTSCOM(RAM_CTRL, pls_core, RAM_MFSPR_PSSCR_GPR0 | (((uint64_t) thread) << 62));
 
                     PK_TRACE("PSSCR RAM: mtspr sprd , gpr0 via RAM_CTRL");
-                    CME_PUTSCOM(RAM_CTRL, CME_MASK_C0, RAM_MTSPR_SPRD_GPR0 | (((uint64_t) thread) << 62));
-                    CME_GETSCOM(SCRATCH0, CME_MASK_C0, scom_data.value);
+                    CME_PUTSCOM(RAM_CTRL, pls_core, RAM_MTSPR_SPRD_GPR0 | (((uint64_t) thread) << 62));
+
+                    if (pls_core & CME_MASK_C0)
+                    {
+                        CME_GETSCOM(SCRATCH0, CME_MASK_C0, scom_data.value);
+                        G_pls[0][thread] = (scom_data.words.upper & BITS32(0, 4)) >> SHIFT32(3);
+                    }
 
-                    G_pls[0][thread] = (scom_data.words.upper & BITS32(0, 4)) >> SHIFT32(3);
 #ifdef PLS_DEBUG
                     PKTRACE("PSSCR %X %X c0thread %X", scom_data.words.upper, scom_data.words.lower, thread);
 #endif
-                }
-            }
 
-            if ((core & CME_MASK_C1) && G_cme_stop_record.req_level[1] > STOP_LEVEL_3)
-            {
-                for (thread = 0; thread < 4; thread++)
-                {
-                    PK_TRACE("PSSCR RAM: mfspr psscr, gpr0 via RAM_CTRL");
-                    CME_PUTSCOM(RAM_CTRL, CME_MASK_C1, RAM_MFSPR_PSSCR_GPR0 | (((uint64_t) thread) << 62));
-
-                    PK_TRACE("PSSCR RAM: mtspr sprd , gpr0 via RAM_CTRL");
-                    CME_PUTSCOM(RAM_CTRL, CME_MASK_C1, RAM_MTSPR_SPRD_GPR0 | (((uint64_t) thread) << 62));
-                    CME_GETSCOM(SCRATCH1, CME_MASK_C1, scom_data.value);
+                    if (pls_core & CME_MASK_C1)
+                    {
+                        CME_GETSCOM(SCRATCH1, CME_MASK_C1, scom_data.value);
+                        G_pls[1][thread] = (scom_data.words.upper & BITS32(0, 4)) >> SHIFT32(3);
+                    }
 
-                    G_pls[1][thread] = (scom_data.words.upper & BITS32(0, 4)) >> SHIFT32(3);
 #ifdef PLS_DEBUG
                     PKTRACE("PSSCR %X %X c1thread %X", scom_data.words.upper, scom_data.words.lower, thread);
 #endif
                 }
-            }
 
-            if (target_level > STOP_LEVEL_3 || deeper_level > STOP_LEVEL_3)
-            {
                 PK_TRACE("RAMMING Disable thread0-3 for RAM via THREAD_INFO");
                 CME_PUTSCOM(THREAD_INFO, core, 0);
 
@@ -786,9 +780,11 @@ p9_cme_stop_entry()
 
                 PK_TRACE("RAMMING Clear core maintenance mode via direct controls");
                 CME_PUTSCOM(DIRECT_CONTROLS, core, (BIT64(3) | BIT64(11) | BIT64(19) | BIT64(27)));
+
+                sync();
+
             }
 
-            sync();
 #endif
 
 
@@ -1372,7 +1368,7 @@ p9_cme_stop_entry()
 
                 CME_GETSCOM_OR( CPPM_CSAR, core, scom_data.value );
 
-                if( CME_STOP_HCODE_ERR_INJ_BIT & scom_data.words.upper )
+                if( BIT64(CPPM_CSAR_STOP_HCODE_ERROR_INJECT) & scom_data.value )
                 {
                     PK_TRACE_DBG("CME STOP ENTRY ERROR INJECT TRAP");
                     PK_PANIC(CME_STOP_ENTRY_TRAP_INJECT);
@@ -1622,7 +1618,6 @@ p9_cme_stop_entry()
             }
 
             sync();
-
             PK_TRACE("Clear special wakeup after wakeup_notify = 1 since it is edge triggered");
             out32(CME_LCL_EISR_CLR, core << SHIFT32(15));
 
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_irq_handlers.c b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_irq_handlers.c
index 6ca903b0..461813dd 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_irq_handlers.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_irq_handlers.c
@@ -39,9 +39,8 @@ extern CmeRecord       G_cme_record;
 #endif
 
 void
-p9_cme_stop_pcwu_handler(void* arg, PkIrqId irq)
+p9_cme_stop_pcwu_handler(void)
 {
-    PkMachineContext ctx __attribute__((unused));
     uint32_t  core_mask = 0;
     uint32_t  core      = (in32(CME_LCL_EISR) & BITS32(12, 2)) >> SHIFT32(13);
     data64_t  scom_data = {0};
@@ -71,7 +70,8 @@ p9_cme_stop_pcwu_handler(void* arg, PkIrqId irq)
                 }
 
                 // block pc for stop8,11 or stop5 as pig sent
-                g_eimr_override |= (IRQ_VEC_PCWU_C0 >> (core_mask & 1));
+                // use 32 bit UPPER mask to prevent compiler from doing 64-bit shifting
+                g_eimr_override |= ((uint64_t)(IRQ_VEC_PCWU_C0_UPPER >> (core_mask & 1))) << 32 | 0x00000000;
                 G_cme_stop_record.core_blockpc |= core_mask;
                 core = core - core_mask;
             }
@@ -81,19 +81,14 @@ p9_cme_stop_pcwu_handler(void* arg, PkIrqId irq)
     // if still wakeup for core with notify_select == cme, go exit
     if (core)
     {
+        PK_TRACE_INF("Launching exit thread");
+
         out32(CME_LCL_EIMR_OR, BITS32(12, 10));
-#if defined(__IOTA__)
         wrteei(1);
         p9_cme_stop_exit();
-        // re-evaluate g_eimr_override then restore eimr
+
+        // re-evaluate stop entry & exit enables
         p9_cme_stop_eval_eimr_override();
-#else
-        pk_semaphore_post((PkSemaphore*)arg);
-#endif
-    }
-    else
-    {
-        pk_irq_vec_restore(&ctx);
     }
 }
 
@@ -108,10 +103,9 @@ p9_cme_stop_pcwu_handler(void* arg, PkIrqId irq)
 // 4) Otherwise flip polarity of Special Wakeup in EISR and clear PM_EXIT
 //    (note for abort cases do not Do not flip polarity of Special Wakeup in EISR.)
 void
-p9_cme_stop_spwu_handler(void* arg, PkIrqId irq)
+p9_cme_stop_spwu_handler(void)
 {
-    PkMachineContext ctx __attribute__((unused));
-    int      sem_post   = 0;
+    int      spwu_rise   = 0;
     uint32_t core_mask  = 0;
     uint32_t core_index = 0;
     uint32_t raw_spwu   = (in32(CME_LCL_EISR) & BITS32(14, 2)) >> SHIFT32(15);
@@ -128,7 +122,7 @@ p9_cme_stop_spwu_handler(void* arg, PkIrqId irq)
             core_index = core_mask & 1;
             PK_TRACE("Detect SPWU signal level change on core%d", core_index);
 
-            // if failing edge == spwu drop:
+            // if falling edge == spwu drop:
             if (G_cme_stop_record.core_in_spwu & core_mask)
             {
                 PK_TRACE("Falling edge of spwu, first clearing EISR");
@@ -161,82 +155,73 @@ p9_cme_stop_spwu_handler(void* arg, PkIrqId irq)
                         // if in block entry mode, do not release the mask
                         if (!(G_cme_stop_record.core_blockey & core_mask))
                         {
-                            g_eimr_override &= ~(IRQ_VEC_STOP_C0 >> core_index);
+                            // use 32 bit UPPER mask to prevent compiler from doing 64-bit shifting
+                            g_eimr_override &=  ((uint64_t)((~IRQ_VEC_STOP_C0_UPPER) >> core_index)) << 32 | 0xFFFFFFFF;
                         }
                     }
                 }
             }
-            // raising edge, Note do not clear EISR as thread will read and clear:
+            // rising edge, do not clear EISR since thread will read and clear:
             else
             {
-                PK_TRACE("Raising edge of spwu, clear EISR later in exit thread");
-                sem_post = 1;
+                PK_TRACE("Rising edge of spwu, clear EISR later in exit thread");
+                spwu_rise = 1;
             }
         }
     }
 
-    if (sem_post)
+    if (spwu_rise)
     {
-        out32(CME_LCL_EIMR_OR, BITS32(12, 10));
         PK_TRACE_INF("Launching exit thread");
-#if defined(__IOTA__)
+
+        out32(CME_LCL_EIMR_OR, BITS32(12, 10));
         wrteei(1);
         p9_cme_stop_exit();
-        // re-evaluate g_eimr_override then restore eimr
+
+        // re-evaluate stop entry & exit enables
         p9_cme_stop_eval_eimr_override();
-#else
-        pk_semaphore_post((PkSemaphore*)arg);
-#endif
-    }
-    else
-    {
-        pk_irq_vec_restore(&ctx);
     }
 }
 
 
 
 void
-p9_cme_stop_rgwu_handler(void* arg, PkIrqId irq)
+p9_cme_stop_rgwu_handler(void)
 {
     MARK_TRAP(STOP_RGWU_HANDLER)
     PK_TRACE_INF("RGWU Handler Trigger");
+
     out32(CME_LCL_EIMR_OR, BITS32(12, 10));
-#if defined(__IOTA__)
     wrteei(1);
     p9_cme_stop_exit();
-    // re-evaluate g_eimr_override then restore eimr
+
+    // re-evaluate stop entry & exit enables
     p9_cme_stop_eval_eimr_override();
-#else
-    pk_semaphore_post((PkSemaphore*)arg);
-#endif
 }
 
 
 
 void
-p9_cme_stop_enter_handler(void* arg, PkIrqId irq)
+p9_cme_stop_enter_handler(void)
 {
     MARK_TRAP(STOP_ENTER_HANDLER)
     PK_TRACE_INF("PM_ACTIVE Handler Trigger");
+
     out32(CME_LCL_EIMR_OR, BITS32(12, 10));
-#if defined(__IOTA__)
     wrteei(1);
+
     // The actual entry sequence
     p9_cme_stop_entry();
-    // re-evaluate g_eimr_override then restore eimr
+
+    // re-evaluate stop entry & exit enables
     p9_cme_stop_eval_eimr_override();
-#else
-    pk_semaphore_post((PkSemaphore*)arg);
-#endif
 }
 
 
 
 void
-p9_cme_stop_db2_handler(void* arg, PkIrqId irq)
+p9_cme_stop_db2_handler(void)
 {
-    PkMachineContext ctx __attribute__((unused));
     cppm_cmedb2_t    db2 = {0};
     ppm_pig_t        pig = {0};
     uint32_t         core = (in32(CME_LCL_EISR) & BITS32(18, 2)) >> SHIFT32(19);
@@ -305,7 +290,7 @@ p9_cme_stop_db2_handler(void* arg, PkIrqId irq)
                         p9_cme_resclk_update(ANALOG_COMMON, p9_cme_resclk_get_index(G_cme_pstate_record.quadPstate),
                                              G_cme_pstate_record.resclkData.common_resclk_idx);
 
-                        // reenable pstate from changing resonent clock
+                        // reenable pstate from changing resonant clock
                         out32(CME_LCL_FLAGS_OR, BIT32(CME_FLAGS_RCLK_OPERABLE));
                         // clear abort flag to start clean slate
                         G_ndd20_disable_stop8_abort_stop11_rclk_handshake_flag = 0;
@@ -324,39 +309,26 @@ p9_cme_stop_db2_handler(void* arg, PkIrqId irq)
             }
         }
     }
-
-    pk_irq_vec_restore(&ctx);
 }
 
 
 
 void
-p9_cme_stop_db1_handler(void* arg, PkIrqId irq)
+p9_cme_stop_db1_handler(void)
 {
-    PkMachineContext ctx __attribute__((unused));
     cppm_cmedb1_t    db1         = {0};
     ppm_pig_t        pig         = {0};
-    uint32_t         core        = 0;
     uint32_t         suspend_ack = 0;
 
     MARK_TRAP(STOP_DB1_HANDLER)
     PK_TRACE_INF("DB1 Handler Trigger");
 
     // Suspend DB should only come from the first good core
-    if (G_cme_record.core_enabled & CME_MASK_C0)
-    {
-        CME_GETSCOM(CPPM_CMEDB1, CME_MASK_C0, db1.value);
-        CME_PUTSCOM_NOP(CPPM_CMEDB1, CME_MASK_C0, 0);
-        out32_sh(CME_LCL_EISR_CLR, BIT64SH(40));
-        core = CME_MASK_C0;
-    }
-    else if (G_cme_record.core_enabled & CME_MASK_C1)
-    {
-        CME_GETSCOM(CPPM_CMEDB1, CME_MASK_C1, db1.value);
-        CME_PUTSCOM_NOP(CPPM_CMEDB1, CME_MASK_C1, 0);
-        out32_sh(CME_LCL_EISR_CLR, BIT64SH(41));
-        core = CME_MASK_C1;
-    }
+    uint32_t core = G_cme_pstate_record.firstGoodCoreMask;
+
+    CME_GETSCOM(CPPM_CMEDB1, core, db1.value);
+    CME_PUTSCOM_NOP(CPPM_CMEDB1, core, 0);
+    out32_sh(CME_LCL_EISR_CLR, core << SHIFT64SH(41));
 
     // block msgs
     if ((db1.fields.cme_message_numbern > 0x4) &&
@@ -453,6 +425,4 @@ p9_cme_stop_db1_handler(void* arg, PkIrqId irq)
         pig.fields.req_intr_type    = PIG_TYPE2;
         CME_PUTSCOM_NOP(PPM_PIG, core, pig.value);
     }
-
-    pk_irq_vec_restore(&ctx);
 }
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_threads.c b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_threads.c
index 8c60174f..e130cca4 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_threads.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_threads.c
@@ -29,7 +29,14 @@
 extern CmeStopRecord G_cme_stop_record;
 extern CmeRecord G_cme_record;
 
-
+// NDD2: OOB bits wired to SISR
+//       not implemented in DD1
+// bit0 is System checkstop
+// bit1 is Recoverable Error
+// bit2 is Special Attention
+// bit3 is Core Checkstop
+#define bad_error_present (((in32   (CME_LCL_SISR) & (  BIT32(12) |   BITS32(14,2)))) || \
+                           ((in32_sh(CME_LCL_SISR) & (BIT64SH(60) | BITS64SH(62,2)))))
 
 void
 p9_cme_stop_core_error_handler(uint32_t core, uint32_t core_error, uint32_t panic_code)
@@ -80,57 +87,345 @@ p9_cme_stop_eval_eimr_override()
     g_eimr_override |= mask_irqs.value;
 }
 
-
-
+#if !DISABLE_PERIODIC_CORE_QUIESCE && (NIMBUS_DD_LEVEL == 20 || NIMBUS_DD_LEVEL == 21 || CUMULUS_DD_LEVEL == 10)
+extern CmeRecord G_cme_fit_record;
 void
-p9_cme_stop_enter_thread(void* arg)
+p9_cme_core_livelock_buster()
 {
-    PkMachineContext  ctx __attribute__((unused));
 
-#if !defined(__IOTA__)
+    uint32_t core_quiesce_cpmmr_disable;
+    uint32_t core;
+    uint32_t core_instr_running;
+    uint32_t scom_op;
+    data64_t scom_data;
+
+#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
 
-    while(1)
+    scom_op = CME_SCOM_NOP;
+
+    for(core = CME_MASK_C0; core > 0; core--)
     {
-        // Thread goes to sleep
-        pk_semaphore_pend(&(G_cme_stop_record.sem[0]), PK_WAIT_FOREVER);
-        wrteei(1);
 
-        // The actual entry sequence
-        p9_cme_stop_entry();
+#else
+
+    scom_op = CME_SCOM_OR;
+    core    = CME_MASK_BC;
+
+#endif
+
+        CME_GETSCOM_OP(CPPM_CPMMR, core, scom_op, scom_data.value);
+        core_quiesce_cpmmr_disable = scom_data.words.upper & BIT32(2);
+
+#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
 
-        // re-evaluate g_eimr_override then restore eimr
-        p9_cme_stop_eval_eimr_override();
-        pk_irq_vec_restore(&ctx);
     }
 
 #endif
-}
 
+    if (!core_quiesce_cpmmr_disable)
+    {
+        out32(CME_LCL_FLAGS_OR,  BIT32(CME_FLAGS_CORE_QUIESCE_ACTIVE));
+    }
+    else
+    {
+        out32(CME_LCL_FLAGS_CLR, BIT32(CME_FLAGS_CORE_QUIESCE_ACTIVE));
+    }
+
+    // only run workaround if
+    // 1) both cores are enabled
+    // 2) at least one core is running
+    //    (stop entry clears the counter)
+    // 3) both cores doesnt have special_wakeup_done asserted
+    //    (spwu_done clears the counter)
+    // 4) both core doesnt have cpmmr[2] asserted
+    // 5) no bad error occurs
 
+    // Get instruction running per core
+    core_instr_running = (in32_sh(CME_LCL_SISR) >> SHIFT64SH(47))& CME_MASK_BC;
 
-void
-p9_cme_stop_exit_thread(void* arg)
+    if((G_cme_record.core_enabled == CME_MASK_BC) &&
+       (core_instr_running != 0) &&
+       (!(in32(CME_LCL_SISR) & BITS32(16, 2))) &&
+       (!core_quiesce_cpmmr_disable) &&
+       (!bad_error_present))
+    {
+        if (G_cme_fit_record.core_quiesce_fit_trigger < 10)
+        {
+            G_cme_fit_record.core_quiesce_fit_trigger++;
+        }
+        else
+        {
+            G_cme_fit_record.core_quiesce_fit_trigger = 0;
+            periodic_core_quiesce_workaround(core_instr_running);
+        }
+    }
+}
+
+
+inline static
+void periodic_core_quiesce_workaround(uint32_t core_instruction_running)
 {
-    PkMachineContext  ctx __attribute__((unused));
+    uint32_t core;
+    uint32_t core_accessible;
+    uint32_t fused_core_mode;
+    uint32_t spattn_offset;
+    uint32_t spattn[2];
+    uint32_t maint_mode[2];
+    uint32_t time_stamp[2];
+    data64_t scom_data;
+    uint32_t sample_error = 0;
+    uint32_t saved_msr = 0;
+
+    CME_GETSCOM_AND(CPPM_CPMMR, CME_MASK_BC, scom_data.value);
+    fused_core_mode = scom_data.words.upper & BIT32(9);
+
+    PK_TRACE("PCQW: Fused Core Mode[%x]", fused_core_mode);
+
+    //0) in case in stop0/1 that we dont know about
+
+    PK_TRACE("PCQW: Assert block interrupt to PC via SICR[2/3]");
+    out32(CME_LCL_SICR_OR, core_instruction_running << SHIFT32(3));
 
-    //--------------------------------------------------------------------------
-    // Starting Thread Loop
-    //--------------------------------------------------------------------------
-#if !defined(__IOTA__)
+    PK_TRACE("PCQW: Waking up the core(pm_exit=1) via SICR[4/5]");
+    out32(CME_LCL_SICR_OR, core_instruction_running << SHIFT32(5));
 
-    while(1)
+    CME_PM_EXIT_DELAY
+
+    PK_TRACE("PCQW: Polling for core wakeup(pm_active=0) via EINR[20/21]");
+
+    while((in32(CME_LCL_EINR)) & (core_instruction_running << SHIFT32(21)));
+
+    //1) Acquire Pcb Mux
+
+    core_accessible = ((~in32(CME_LCL_SISR)) >> SHIFT32(11)) & core_instruction_running;
+
+    PK_TRACE("PCQW: Request PCB Mux via SICR[10/11]");
+    out32(CME_LCL_SICR_OR, core_accessible << SHIFT32(11));
+
+    // Poll Infinitely for PCB Mux Grant
+    while((core_accessible & (in32(CME_LCL_SISR) >> SHIFT32(11))) != core_accessible);
+
+    PK_TRACE("PCQW: PCB Mux Granted");
+
+    //2) Read RAS_STATUS Scom Addr(20:31) = x0A02
+    //   bit (0 + 8*T) where (T= thread) CORE_MAINT_MODE
+    //   to find out which threads are in maintenance mode
+
+    // vector = 0 + 8*T as a shifting base used below
+#define THREAD_VECTOR (BIT32(0)|BIT32(8)|BIT32(16)|BIT32(24))
+
+    for(core = CME_MASK_C0; core > 0; core--)
     {
-        // Thread goes to sleep
-        pk_semaphore_pend(&(G_cme_stop_record.sem[1]), PK_WAIT_FOREVER);
-        wrteei(1);
+        if (core & core_instruction_running)
+        {
+            CME_GETSCOM(RAS_STATUS, core, scom_data.value) ;
+            maint_mode[core & 1] = scom_data.words.upper & THREAD_VECTOR;
+        }
+    }
+
+
+    //3) Write DIRECT_CONTROLS Scom Addr(20:31) = x0A9C
+    //   bit (7 + 8*T) where (T= thread) DC_CORE_STOP for ALL threads.
+    //   This will quiesce the active threads,
+    //   put all threads into core maintenance mode,
+    //   and eventually quiesce the entire core.
 
-        // The actual exit sequence
-        p9_cme_stop_exit();
+    scom_data.words.lower = 0;
+    scom_data.words.upper = THREAD_VECTOR >> 7;
 
-        // re-evaluate g_eimr_override then restore eimr
-        p9_cme_stop_eval_eimr_override();
-        pk_irq_vec_restore(&ctx);
+#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
+
+    for(core = CME_MASK_C0; core > 0; core--)
+    {
+        if (core & core_instruction_running)
+        {
+#else
+
+    core = core_instruction_running;
+
+#endif
+
+            // The SCOM can be delayed by traffic on PC on the SPR bus, so it is possible
+            // to get a RC=4 (Address Error), which really indicates a timeout.   Need to mask
+            // this return code and retry until we get a clean return code
+            saved_msr = mfmsr();
+            mtmsr( saved_msr | MSR_SEM4);  // Mask off timeout
+
+            do
+            {
+                CME_PUTSCOM_NOP(DIRECT_CONTROLS, core, scom_data.value);
+            }
+            while ((mfmsr() & MSR_SIBRC) != 0);
+
+            mtmsr(saved_msr);
+
+#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
+        }
     }
 
 #endif
+
+
+    //4) Loop on RAS_STATUS Scom Addr(20:31) = x0A02
+    //   until bit(1 + 8*T) THREAD_QUIESCE are all active b1
+
+    time_stamp[0] = in32(CME_LCL_TBR);
+
+#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
+
+    for(core = CME_MASK_C0; core > 0; core--)
+    {
+        if (core & core_instruction_running)
+        {
+#else
+
+    core = core_instruction_running;
+
+#endif
+
+#define THREAD_VECTOR_CHECK (THREAD_VECTOR>>1 | THREAD_VECTOR>>3)
+
+// In a future release of this patch, it should be based on the Nest Frequency, but
+// plumbing for that sill needs to be created.
+// 200us in 32ns timer ticks
+#define QUIESCE_ABORT_TICKS 0x186A
+
+            // Poll on THREAD_QUIESCE, LSU_QUIESCE, and NEST_ACTIVE.
+            // If they do not quiesce in 200us abort the patch and restart the cores.
+
+            do
+            {
+                CME_GETSCOM_AND(RAS_STATUS, core, scom_data.value);
+
+                time_stamp[1] = in32(CME_LCL_TBR);
+
+                if (time_stamp[1] > time_stamp[0])
+                {
+                    G_cme_fit_record.core_quiesce_time_latest =
+                        time_stamp[1] - time_stamp[0];
+                }
+                else
+                {
+                    G_cme_fit_record.core_quiesce_time_latest =
+                        0xFFFFFFFF - time_stamp[0] + time_stamp[1] + 1;
+                }
+            }
+            while((((scom_data.words.upper& THREAD_VECTOR_CHECK) != THREAD_VECTOR_CHECK)
+                   ||    //THREAD_ and LSU_QUIESCE must be ones
+                   ((scom_data.words.lower& BIT64SH(32))))  // NEST_ACTIVE must be zero
+                  && !(sample_error = bad_error_present)
+                  && (G_cme_fit_record.core_quiesce_time_latest < QUIESCE_ABORT_TICKS)    // 200us in 32ns timer ticks
+                 );
+
+
+#if NIMBUS_DD_LEVEL == 20 || DISABLE_CME_DUAL_CAST == 1
+        }
+    }
+
+#endif
+
+    if (!sample_error && (G_cme_fit_record.core_quiesce_time_latest < QUIESCE_ABORT_TICKS) )
+    {
+        PK_TRACE("PCQW: Both Cores Quiesced");
+    }
+    else
+    {
+        PK_TRACE_INF("PCQW: Error while trying to Quiesce Cores.  Bad Error %d, QuiesceTime (ns) %d", sample_error,
+                     (G_cme_fit_record.core_quiesce_time_latest << 5));
+        G_cme_fit_record.core_quiesce_failed_count++;
+    }
+
+
+    for(core = CME_MASK_C0; core > 0; core--)
+    {
+        if (core & core_instruction_running)
+        {
+            //5) Read SPATTN Scom Addr(20:31) = x0A98 to check for ATTN
+            //   (need to do this after all threads quiesce to close windows)
+
+            CME_GETSCOM(SPATTN_READ, core, scom_data.value);
+
+            if (fused_core_mode)
+            {
+                //Fused Core Mode
+                // C0 vtid0=ltid0 bit1  -> tv.bit0
+                // C0 vtid1=ltid2 bit9  -> tv.bit8
+                // C0 vtid2=ltid4 bit17 -> tv.bit16
+                // C0 vtid3=ltid6 bit25 -> tv.bit24
+                //
+                // C1 vtid0=ltid1 bit5  -> tv.bit0
+                // C1 vtid1=ltid3 bit13 -> tv.bit8
+                // C1 vtid2=ltid5 bit21 -> tv.bit16
+                // C1 vtid3=ltid7 bit29 -> tv.bit24
+                spattn_offset = ((core & 1) << 2) + 1; // C0:1, C1:5
+                spattn[core & 1] = ((scom_data.words.upper & BIT32((0 + spattn_offset))) << spattn_offset) | //0
+                                   ((scom_data.words.upper & BIT32((8 + spattn_offset))) << spattn_offset) | //8
+                                   ((scom_data.words.upper & BIT32((16 + spattn_offset))) << spattn_offset) | //16
+                                   ((scom_data.words.upper & BIT32((24 + spattn_offset))) << spattn_offset); //24
+            }
+            else
+            {
+                // Normal Mode
+                // vtid0=ltid0 bit1  -> tv.bit0
+                // vtid1=ltid1 bit5  -> tv.bit8
+                // vtid2=ltid2 bit9  -> tv.bit16
+                // vtid3=ltid3 bit13 -> tv.bit24
+                spattn[core & 1] = ((scom_data.words.upper & BIT32(1))  << 1 ) | //0
+                                   ((scom_data.words.upper & BIT32(5))  >> 3 ) | //8
+                                   ((scom_data.words.upper & BIT32(9))  >> 7 ) | //16
+                                   ((scom_data.words.upper & BIT32(13)) >> 11);  //24
+            }
+
+
+            //6) Write DIRECT_CONTROLS Scom Addr(20:31) = x0A9C
+            //   bit (3 + 8*T) where (T= thread) DC_CLEAR_MAINT for all threads
+            //   which were not in maintenance mode in step 1 AND do not have ATTN set in step 4
+
+            scom_data.words.lower = 0;
+            scom_data.words.upper =
+                (THREAD_VECTOR & (~maint_mode[core & 1]) & (~spattn[core & 1])) >> 3;
+            CME_PUTSCOM_NOP(DIRECT_CONTROLS, core, scom_data.value);
+        }
+    }
+
+    PK_TRACE("PCQW: Both Cores Started");
+
+    //7) Drop pm_exit
+
+    PK_TRACE("PCQW: Drop pm_exit via SICR[4/5]");
+    out32(CME_LCL_SICR_CLR, core_instruction_running << SHIFT32(5));
+
+    PK_TRACE("PCQW: Drop block interrupt to PC via SICR[2/3]");
+    out32(CME_LCL_SICR_CLR, core_instruction_running << SHIFT32(3));
+
+    //8) Release Pcb Mux on Both Cores
+
+    PK_TRACE("PCQW: Release PCB Mux back on Both Cores via SICR[10/11]");
+    out32(CME_LCL_SICR_CLR, core_accessible << SHIFT32(11));
+
+    while((core_accessible & ~(in32(CME_LCL_SISR) >> SHIFT32(11))) != core_accessible);
+
+    PK_TRACE("PCQW: PCB Mux Released on Both Cores");
+
+
+    G_cme_fit_record.core_quiesce_total_count += 1;
+
+    //Profile time
+
+    // timestamp delta was computed above to handle the abort case
+
+    if (G_cme_fit_record.core_quiesce_time_latest <
+        G_cme_fit_record.core_quiesce_time_min)
+    {
+        G_cme_fit_record.core_quiesce_time_min =
+            G_cme_fit_record.core_quiesce_time_latest;
+    }
+    else if (G_cme_fit_record.core_quiesce_time_latest >
+             G_cme_fit_record.core_quiesce_time_max)
+    {
+        G_cme_fit_record.core_quiesce_time_max =
+            G_cme_fit_record.core_quiesce_time_latest;
+    }
 }
+
+#endif