CME/SGPE: STOP Images functional bug fix collection

Change-Id: I3858a93ade6f21c006c980a497f58d6dafd9b628
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/27000
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Tested-by: PPE CI <ppe-ci+hostboot@us.ibm.com>
Reviewed-by: Douglas R. Gilbert <dgilbert@us.ibm.com>
Reviewed-by: Gregory S. Still <stillgs@us.ibm.com>

13 files changed, 215 insertions, 115 deletions

diff --git a/import/chips/p9/common/pmlib/include/gpehw_common.h b/import/chips/p9/common/pmlib/include/gpehw_common.h
index 0349bad9..6465983b 100644
--- a/import/chips/p9/common/pmlib/include/gpehw_common.h
+++ b/import/chips/p9/common/pmlib/include/gpehw_common.h
@@ -91,6 +91,7 @@ enum GPE_SCOM_ADDRESS_PARAMETERS
 #define GPE_GETSCOM(addr, data)   getscom(0, addr, &data);
 
 #define GPE_PUTSCOM(addr, data)   putscom(0, addr, data);
+
 #define GPE_GETSCOM_VAR(addr, cplt_base, cq_offset, ex_select, data) \
     getscom(0,GPE_SCOM_ADDR(addr, cplt_base, cq_offset, ex_select),&data);
 
diff --git a/import/chips/p9/common/pmlib/include/p9_stop_common.h b/import/chips/p9/common/pmlib/include/p9_stop_common.h
index 6f14eba5..a7d6b2c0 100644
--- a/import/chips/p9/common/pmlib/include/p9_stop_common.h
+++ b/import/chips/p9/common/pmlib/include/p9_stop_common.h
@@ -28,12 +28,13 @@ enum P9_HCD_COMMON_CLK_CTRL_CONSTANTS
 {
     CLK_STOP_CMD                             = BIT64(0),
     CLK_START_CMD                            = BIT64(1),
+    CLK_REGION_ANEP                          = BIT64(10),
     CLK_REGION_DPLL                          = BIT64(14),
+    CLK_REGION_EX0_L2_L3_REFR                = BIT64(6) | BIT64(8) | BIT64(12),
+    CLK_REGION_EX1_L2_L3_REFR                = BIT64(7) | BIT64(9) | BIT64(13),
     CLK_REGION_ALL_BUT_EX                    = BITS64(4, 2) | BITS64(10, 2) | BIT64(14),
     CLK_REGION_ALL_BUT_EX_DPLL               = BITS64(4, 2) | BITS64(10, 2),
     CLK_REGION_ALL_BUT_EX_ANEP_DPLL          = BITS64(4, 2) | BIT64(11),
-    CLK_REGION_EX0_L2_L3_REFR                = BIT64(6) | BIT64(8) | BIT64(12),
-    CLK_REGION_EX1_L2_L3_REFR                = BIT64(7) | BIT64(9) | BIT64(13),
     CLK_REGION_ALL_BUT_PLL                   = BITS64(4, 10),
     CLK_REGION_ALL                           = BITS64(4, 11),
     CLK_THOLD_ALL                            = BITS64(48, 3)
diff --git a/import/chips/p9/procedures/ppe/pk/gpe/gpe_common.h b/import/chips/p9/procedures/ppe/pk/gpe/gpe_common.h
index 07a7cc7c..9c5f952c 100644
--- a/import/chips/p9/procedures/ppe/pk/gpe/gpe_common.h
+++ b/import/chips/p9/procedures/ppe/pk/gpe/gpe_common.h
@@ -75,6 +75,7 @@
         _liw        r5, pk_unified_irq_prty_mask_handler
         mtlr        r5
         blrl            // On return, d5 contains task prty irq vec.
+        mfsprg      r3, 0 // In case r3 is modified by unified handler, restore to sprg0
 #else
         _lwzi       %r5, %r5, GPE_GISR0(APPCFG_OCC_INSTANCE_ID)
 #endif
diff --git a/import/chips/p9/procedures/ppe/pk/std/std_common.h b/import/chips/p9/procedures/ppe/pk/std/std_common.h
index b9f330bc..b5cbba90 100644
--- a/import/chips/p9/procedures/ppe/pk/std/std_common.h
+++ b/import/chips/p9/procedures/ppe/pk/std/std_common.h
@@ -64,6 +64,7 @@
         _liw        r5, pk_unified_irq_prty_mask_handler
         mtlr        r5
         blrl            // On return, d5 contains task prty irq vec.
+        mfsprg      r3, 0 // In case r3 is modified by unified handler, restore to sprg0
 #else
         _lvdg       d5, STD_LCL_EISTR    #load the 64bit interrupt status into d5
 #endif        
diff --git a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.h b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.h
index c7bc1c0a..2a9729fe 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.h
@@ -132,5 +132,5 @@ pk_irq_vec_restore(PkMachineContext* context)
         pk_halt();
     }
 
-    pk_critical_section_exit(context);
+    //pk_critical_section_exit(context);
 }
diff --git a/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h b/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h
index 16d3bb02..cacf1a34 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/pk_app_cfg.h
@@ -34,11 +34,13 @@
 /// \brief Application specific overrides go here.
 ///
 
+#define TEST_ONLY_BCE_IRR     0
+
 #define STOP_PRIME            0
 #define SKIP_ABORT            0
 #define SKIP_L2_PURGE_ABORT   0
 #define SKIP_ENTRY_CATCHUP    0
-#define SKIP_EXIT_CATCHUP     1
+#define SKIP_EXIT_CATCHUP     0
 #define SKIP_ARRAYINIT        1
 #define SKIP_SCAN0            1
 #define SKIP_INITF            0
@@ -58,8 +60,9 @@
 #if EPM_P9_TUNING
     #define PK_TRACE_BUFFER_WRAP_MARKER 1
 #endif
-#define PK_TRACE_TIMER_OUTPUT 0
-#define PK_TRACE_ENABLE       1
+#define PK_TRACE_TIMER_OUTPUT  0
+#define PK_TRACE_ENABLE        1
+#define PK_KERNEL_TRACE_ENABLE 1
 
 // --------------------
 
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 046b0601..7360d7a7 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
@@ -118,6 +118,7 @@
 
 /// Macro to update STOP History
 #define CME_STOP_UPDATE_HISTORY(core,gated,trans,req_l,act_l,req_e,act_e) \
+    hist.value                   = 0;                                     \
     hist.fields.stop_gated       = gated;                                 \
     hist.fields.stop_transition  = trans;                                 \
     hist.fields.req_stop_level   = req_l;                                 \
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 d6c86889..d29d4fee 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
@@ -505,75 +505,79 @@ p9_cme_stop_entry()
         //----------------------------------------------------------------------
         // STOP LEVEL 3
         //----------------------------------------------------------------------
-        /*
-                if (target_level == 3)
-                {
 
-                    //==========================
-                    MARK_TAG(SE_CORE_VMIN, core)
-                    //==========================
+        if (target_level == 3)
+        {
 
-                    PK_TRACE("SE3.a");
-                    // Enable IVRM if not already
+            //==========================
+            MARK_TAG(SE_CORE_VMIN, core)
+            //==========================
 
-                    PK_TRACE("SE3.b");
-                    // Drop to Vmin
+            PK_TRACE("SE3.a");
+            // Enable IVRM if not already
 
-                    if(core & CME_MASK_C0)
-                    {
-                        G_cme_stop_record.act_level_c0 = STOP_LEVEL_3;
-                    }
+            PK_TRACE("SE3.b");
 
-                    if(core & CME_MASK_C1)
-                    {
-                        G_cme_stop_record.act_level_c1 = STOP_LEVEL_3;
-                    }
+            // Drop to Vmin
+            /*
+                        if(core & CME_MASK_C0)
+                        {
+                            G_cme_stop_record.act_level_c0 = STOP_LEVEL_3;
+                        }
 
-                    //===========================
-                    MARK_TAG(SE_STOP3_DONE, core)
-                    //===========================
-
-                    PK_TRACE("SE3.c");
-                    // Update Stop History: In Core Stop Level 3
-                    CME_STOP_UPDATE_HISTORY(core,
-                                            STOP_CORE_IS_GATED,
-                                            STOP_TRANS_COMPLETE,
-                                            target_level,
-                                            STOP_LEVEL_3,
-                                            STOP_REQ_DISABLE,
-                                            STOP_ACT_ENABLE);
-
-                    // If both cores targeting different levels
-                    // deeper core should have at least deeper stop level than 3
-                    // only need to modify deeper core history if another one was done
-                    if (deeper_core)
-                    {
-                        CME_STOP_UPDATE_HISTORY(deeper_core,
+                        if(core & CME_MASK_C1)
+                        {
+                            G_cme_stop_record.act_level_c1 = STOP_LEVEL_3;
+                        }
+
+                        //===========================
+                        MARK_TAG(SE_STOP3_DONE, core)
+                        //===========================
+
+                        PK_TRACE("SE3.c");
+                        // Update Stop History: In Core Stop Level 3
+
+                        CME_STOP_UPDATE_HISTORY(core,
                                                 STOP_CORE_IS_GATED,
-                                                STOP_TRANS_ENTRY,
-                                                deeper_level,
-                                                STOP_LEVEL_2,
+                                                STOP_TRANS_COMPLETE,
+                                                target_level,
+                                                STOP_LEVEL_3,
                                                 STOP_REQ_DISABLE,
                                                 STOP_ACT_ENABLE);
-                        // from now on, proceed with only deeper core
-                        core          = deeper_core;
-                        target_level  = deeper_level;
-                        deeper_level  = 0;
-                        deeper_core   = 0;
-                        entry_ongoing = 1;
-                    }
-                    else
-                    {
-                        entry_ongoing = 0;
-                    }
+            */
+            // If both cores targeting different levels
+            // deeper core should have at least deeper stop level than 3
+            // only need to modify deeper core history if another one was done
+            if (deeper_core)
+            {
+                /*
+                                CME_STOP_UPDATE_HISTORY(deeper_core,
+                                                        STOP_CORE_IS_GATED,
+                                                        STOP_TRANS_ENTRY,
+                                                        deeper_level,
+                                                        STOP_LEVEL_2,
+                                                        STOP_REQ_DISABLE,
+                                                        STOP_ACT_ENABLE);
+                */
+                // from now on, proceed with only deeper core
+                core          = deeper_core;
+                target_level  = deeper_level;
+                deeper_level  = 0;
+                deeper_core   = 0;
+                entry_ongoing = 1;
+            }
+            else
+            {
+                entry_ongoing = 0;
+            }
+
+            // If we are done at STOP level 3
+            if (!entry_ongoing)
+            {
+                break;
+            }
+        }
 
-                    // If we are done at STOP level 3
-                    if (!entry_ongoing)
-                    {
-                        break;
-                    }
-                }
-        */
         //----------------------------------------------------------------------
         // STOP LEVEL 4
         //----------------------------------------------------------------------
@@ -748,12 +752,14 @@ p9_cme_stop_entry()
                 if (!core_aborted &&
                     (in32(CME_LCL_EINR) & BITS32(12, 6)))
                 {
-                    if (in32(CME_LCL_EINR) & IRQ_VEC_WAKE_C0)
+                    if (in32(CME_LCL_EINR) &
+                        (((core & CME_MASK_C0) ? BIT32(12) : 0) | BIT32(14) | BIT32(16)))
                     {
                         core_aborted |= CME_MASK_C0;
                     }
 
-                    if (in32(CME_LCL_EINR) & IRQ_VEC_WAKE_C1)
+                    if (in32(CME_LCL_EINR) &
+                        (((core & CME_MASK_C1) ? BIT32(13) : 0) | BIT32(15) | BIT32(17)))
                     {
                         core_aborted |= CME_MASK_C1;
                     }
@@ -780,6 +786,9 @@ p9_cme_stop_entry()
             MARK_TAG(SE_PURGE_L2_DONE, core_aborted ? core_aborted : CME_MASK_BC)
             //===============================================================
 
+            // if core = 3 aborted = 1, core = 2(sgpe handoff) aborted (cme wakeup)
+            // if core = 1 aborted = 1, core = 0(break)        aborted (cme wakeup)
+            // if core = 1 aborted = 2, core = 1(sgpe handoff) aborted (sgpe wakeup)
             if (core !=  core_aborted)
             {
                 core &= ~core_aborted;
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c
index 83cee38d..74880d92 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_exit.c
@@ -32,11 +32,12 @@ extern CmeStopRecord G_cme_stop_record;
 int
 p9_cme_stop_exit()
 {
-    int          d2u4_flag       = 0;
-    int          catchup_ongoing = 0;
+    int          d2u4_flag         = 0;
+    int          catchup_ongoing_a = 0;
+    int          catchup_ongoing_b = 0;
     uint8_t      target_level;
-    uint8_t      deeper_level    = 0;
-    uint32_t     deeper_core     = 0;
+    uint8_t      deeper_level      = 0;
+    uint32_t     deeper_core       = 0;
     uint32_t     wakeup;
     uint32_t     core;
 #if !SPWU_AUTO
@@ -48,6 +49,7 @@ p9_cme_stop_exit()
     uint32_t     core_catchup;
 #endif
     ppm_sshsrc_t hist;
+    uint64_t     data64;
 
     //--------------------------------------------------------------------------
     // BEGIN OF STOP EXIT
@@ -57,6 +59,26 @@ p9_cme_stop_exit()
     wakeup = (in32(CME_LCL_EISR) >> SHIFT32(17)) & 0x3F;
     core   = ((wakeup >> 4) | (wakeup >> 2) | wakeup) & CME_MASK_BC;
 
+    if (core & CME_MASK_C0)
+    {
+        CME_GETSCOM(CPPM_CPMMR, CME_MASK_C0, CME_SCOM_AND, data64);
+
+        if (data64 & BIT64(13))
+        {
+            core = core - CME_MASK_C0;
+        }
+    }
+
+    if (core & CME_MASK_C1)
+    {
+        CME_GETSCOM(CPPM_CPMMR, CME_MASK_C1, CME_SCOM_AND, data64);
+
+        if (data64 & BIT64(13))
+        {
+            core = core - CME_MASK_C1;
+        }
+    }
+
     // override with partial good core mask
     // also ignore wakeup to running cores
     core = core & G_cme_stop_record.core_enabled &
@@ -74,7 +96,7 @@ p9_cme_stop_exit()
 
     if (spwu_wake)
     {
-        out32(CME_LCL_SICR_OR,  spwu << SHIFT32(17));
+        out32(CME_LCL_SICR_OR,  spwu_wake << SHIFT32(17));
     }
 
 #endif
@@ -191,12 +213,12 @@ p9_cme_stop_exit()
 #if !STOP_PRIME
 #if !SKIP_EXIT_CATCHUP
 
-            if (catchup_ongoing)
+            if (catchup_ongoing_a && (!catchup_ongoing_b))
             {
                 core = CME_MASK_BC;
-                catchup_ongoing = 0;
+                catchup_ongoing_a = 0;
             }
-            else
+            else if ((core != CME_MASK_BC) && (!deeper_core) && (!catchup_ongoing_b))
             {
                 wakeup = (in32(CME_LCL_EISR) >> SHIFT32(17)) & 0x3F;
                 core_catchup = (~core) &
@@ -204,6 +226,26 @@ p9_cme_stop_exit()
                 core_catchup = core_catchup & G_cme_stop_record.core_enabled &
                                (~G_cme_stop_record.core_running);
 
+                if (core_catchup & CME_MASK_C0)
+                {
+                    CME_GETSCOM(CPPM_CPMMR, CME_MASK_C0, CME_SCOM_AND, data64);
+
+                    if (data64 & BIT64(13))
+                    {
+                        core_catchup = core_catchup - CME_MASK_C0;
+                    }
+                }
+
+                if (core_catchup & CME_MASK_C1)
+                {
+                    CME_GETSCOM(CPPM_CPMMR, CME_MASK_C1, CME_SCOM_AND, data64);
+
+                    if (data64 & BIT64(13))
+                    {
+                        core_catchup = core_catchup - CME_MASK_C1;
+                    }
+                }
+
                 if (core_catchup)
                 {
                     //==================================
@@ -242,7 +284,7 @@ p9_cme_stop_exit()
                     else
                     {
                         core = core_catchup;
-                        catchup_ongoing = 1;
+                        catchup_ongoing_a = 1;
                         continue;
                     }
                 }
@@ -268,12 +310,12 @@ p9_cme_stop_exit()
 
 #if !SKIP_EXIT_CATCHUP
 
-            if (catchup_ongoing)
+            if (catchup_ongoing_b)
             {
                 core = CME_MASK_BC;
-                catchup_ongoing = 0;
+                catchup_ongoing_b = 0;
             }
-            else
+            else if ((core != CME_MASK_BC) && (!deeper_core))
             {
                 wakeup = (in32(CME_LCL_EISR) >> SHIFT32(17)) & 0x3F;
                 core_catchup = (~core) &
@@ -281,6 +323,26 @@ p9_cme_stop_exit()
                 core_catchup = core_catchup & G_cme_stop_record.core_enabled &
                                (~G_cme_stop_record.core_running);
 
+                if (core_catchup & CME_MASK_C0)
+                {
+                    CME_GETSCOM(CPPM_CPMMR, CME_MASK_C0, CME_SCOM_AND, data64);
+
+                    if (data64 & BIT64(13))
+                    {
+                        core_catchup = core_catchup - CME_MASK_C0;
+                    }
+                }
+
+                if (core_catchup & CME_MASK_C1)
+                {
+                    CME_GETSCOM(CPPM_CPMMR, CME_MASK_C1, CME_SCOM_AND, data64);
+
+                    if (data64 & BIT64(13))
+                    {
+                        core_catchup = core_catchup - CME_MASK_C1;
+                    }
+                }
+
                 if (core_catchup)
                 {
                     //==================================
@@ -319,7 +381,7 @@ p9_cme_stop_exit()
                     else
                     {
                         core = core_catchup;
-                        catchup_ongoing = 1;
+                        catchup_ongoing_b = 1;
                         continue;
                     }
                 }
@@ -345,7 +407,7 @@ p9_cme_stop_exit()
 #endif
 
         }
-        while(catchup_ongoing);
+        while(catchup_ongoing_a || catchup_ongoing_b);
 
         if (d2u4_flag)
         {
@@ -356,18 +418,18 @@ p9_cme_stop_exit()
     //--------------------------------------------------------------------------
     // STOP LEVEL 3
     //--------------------------------------------------------------------------
-    /*
-        if (deeper_level == STOP_LEVEL_3 || target_level == STOP_LEVEL_3)
-        {
-            //======================
-            MARK_TAG(SX_STOP3, core)
-            //======================
 
-            PK_TRACE("STOP Level 3 Sequence");
-            //Return to full voltage
-            //disable ivrm?
-        }
-    */
+    if (deeper_level == STOP_LEVEL_3 || target_level == STOP_LEVEL_3)
+    {
+        //======================
+        MARK_TAG(SX_STOP3, core)
+        //======================
+
+        PK_TRACE("STOP Level 3 Sequence");
+        //Return to full voltage
+        //disable ivrm?
+    }
+
     //--------------------------------------------------------------------------
     // STOP LEVEL 2
     //--------------------------------------------------------------------------
@@ -474,7 +536,7 @@ p9_cme_stop_exit()
 #if EPM_P9_TUNING
             CME_PUTSCOM(SCRACTH0, CME_MASK_C0, 0x200000);
 #else
-            CME_PUTSCOM(SCRACTH0, CME_MASK_C0, int64(SELF_RESTORE_ADDR_FETCH));
+            CME_PUTSCOM(SCRACTH0, CME_MASK_C0, in64(SELF_RESTORE_ADDR_FETCH));
 #endif
         }
 
@@ -487,7 +549,7 @@ p9_cme_stop_exit()
 #if EPM_P9_TUNING
             CME_PUTSCOM(SCRACTH1, CME_MASK_C1, 0x200000);
 #else
-            CME_PUTSCOM(SCRACTH1, CME_MASK_C1, int64(SELF_RESTORE_ADDR_FETCH));
+            CME_PUTSCOM(SCRACTH1, CME_MASK_C1, in64(SELF_RESTORE_ADDR_FETCH));
 #endif
         }
 
@@ -664,7 +726,7 @@ p9_cme_stop_exit()
 
     if (spwu_stop)
     {
-        out32(CME_LCL_SICR_OR,  spwu << SHIFT32(17));
+        out32(CME_LCL_SICR_OR,  spwu_stop << SHIFT32(17));
     }
 
     if ((core = (core & (~spwu_stop))))
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_hcd_core_startclocks.c b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_hcd_core_startclocks.c
index f949a3d2..565f9fa7 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_hcd_core_startclocks.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_hcd_core_startclocks.c
@@ -32,13 +32,16 @@ p9_hcd_core_startclocks(uint32_t core)
     int      rc = CME_STOP_SUCCESS;
     uint64_t scom_data, loop;
 
+    //FAPI_DBG("Set CPLT_CTRL0[AVP_MODE] for cache-contained execution");
+    //FAPI_TRY(putScom(i_target, C_CPLT_CTRL0_OR, MASK_SET(5)));
+
     /// @todo add DD1 attribute control
     PK_TRACE("DD1 only: set sdis_n(flushing LCBES condition workaround");
     CME_PUTSCOM(C_CPLT_CONF0_OR, core, BIT64(34));
 
     PK_TRACE("Set inop_align/wait/wait_cycles via OPCG_ALIGN[0-3,12-19,52-63]");
     CME_GETSCOM(C_OPCG_ALIGN, core, CME_SCOM_AND, scom_data);
-    scom_data = scom_data & ~(BITS64(0, 4) & BITS64(12, 8) & BITS64(52, 12));
+    scom_data = scom_data & ~(BITS64(0, 4) | BITS64(12, 8) | BITS64(52, 12));
     scom_data = scom_data | (BIT64(1) | BIT64(3) | BIT64(59));
     CME_PUTSCOM(C_OPCG_ALIGN, core, scom_data);
 
@@ -77,10 +80,11 @@ p9_hcd_core_startclocks(uint32_t core)
 
     PK_TRACE("Set then unset clear_chiplet_is_aligned via SYNC_CONFIG[7]");
     CME_GETSCOM(C_SYNC_CONFIG, core, CME_SCOM_AND, scom_data);
-    scom_data = scom_data | BIT64(7);
+    scom_data = scom_data | BITS64(7, 2);
     CME_PUTSCOM(C_SYNC_CONFIG, core, scom_data);
     scom_data = scom_data & ~BIT64(7);
     CME_PUTSCOM(C_SYNC_CONFIG, core, scom_data);
+
     // 255 cache cycles
     PPE_WAIT_CORE_CYCLES(loop, 510);
 
diff --git a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop.h b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop.h
index 805fc212..fa1bed51 100644
--- a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop.h
+++ b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop.h
@@ -107,6 +107,7 @@
 
 /// Macro to update STOP History
 #define SGPE_STOP_UPDATE_HISTORY(id,base,gated,trans,req_l,act_l,req_e,act_e) \
+    hist.value                   = 0;                                         \
     hist.fields.stop_gated       = gated;                                     \
     hist.fields.stop_transition  = trans;                                     \
     hist.fields.req_stop_level   = req_l;                                     \
@@ -132,7 +133,7 @@ enum SGPE_STOP_IRQ_PAYLOAD_MASKS
     TYPE2_PAYLOAD_EXIT_EVENT          = 0xC00,
     TYPE2_PAYLOAD_STOP_LEVEL          = 0xF,
     TYPE3_PAYLOAD_EXIT_EVENT          = 0xC00,
-    TYPE6_PAYLOAD_EXIT_EVENT          = 0xC00
+    TYPE6_PAYLOAD_EXIT_EVENT          = 0xF
 };
 
 enum SGPE_STOP_EVENT_LEVELS
@@ -141,6 +142,15 @@ enum SGPE_STOP_EVENT_LEVELS
     LEVEL_EQ_BASE                     = 11
 };
 
+enum SGPE_STOP_CME_FLAGS
+{
+    CME_CORE0_ENTRY_FIRST             = BIT64(28),
+    CME_CORE1_ENTRY_FIRST             = BIT64(29),
+    CME_CORE0_ENABLE                  = BIT64(30),
+    CME_CORE1_ENABLE                  = BIT64(31),
+    CME_EX1_INDICATOR                 = BIT64(26)
+};
+
 enum SGPE_STOP_PSCOM_MASK
 {
     PSCOM_MASK_ALL_L2                 = BITS64(2, 2) | BITS64(10, 2),
diff --git a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c
index 13fcd08f..39932f43 100644
--- a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c
+++ b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_exit.c
@@ -291,25 +291,25 @@ p9_sgpe_stop_exit()
 
                 if (ccsr.value & BIT32((qloop << 2)))
                 {
-                    cme_flags |= 0x2;
+                    cme_flags |= CME_CORE0_ENABLE;
                     GPE_GETSCOM(GPE_SCOM_ADDR_CORE(C_NET_CTRL0,
                                                    ((qloop << 2))),     scom_data);
 
                     if (!(scom_data & BIT64(18)))
                     {
-                        cme_flags |= 0x8;
+                        cme_flags |= CME_CORE0_ENTRY_FIRST;
                     }
                 }
 
                 if (ccsr.value & BIT32((qloop << 2) + 1))
                 {
-                    cme_flags |= 0x1;
+                    cme_flags |= CME_CORE1_ENABLE;
                     GPE_GETSCOM(GPE_SCOM_ADDR_CORE(C_NET_CTRL0,
                                                    ((qloop << 2) + 1)), scom_data);
 
                     if (!(scom_data & BIT64(18)))
                     {
-                        cme_flags |= 0x4;
+                        cme_flags |= CME_CORE1_ENTRY_FIRST;
                     }
                 }
 
@@ -319,29 +319,29 @@ p9_sgpe_stop_exit()
 
             if (m_pg & SND_EX_IN_QUAD)
             {
-                cme_flags = 0;
+                cme_flags = CME_EX1_INDICATOR;
 
                 if (ccsr.value & BIT32(((qloop << 2) + 2)))
                 {
-                    cme_flags |= 0x2;
+                    cme_flags |= CME_CORE0_ENABLE;
                     GPE_GETSCOM(GPE_SCOM_ADDR_CORE(C_NET_CTRL0,
                                                    ((qloop << 2) + 2)), scom_data);
 
                     if (!(scom_data & BIT64(18)))
                     {
-                        cme_flags |= 0x8;
+                        cme_flags |= CME_CORE0_ENTRY_FIRST;
                     }
                 }
 
                 if (ccsr.value & BIT32(((qloop << 2) + 3)))
                 {
-                    cme_flags |= 0x1;
+                    cme_flags |= CME_CORE1_ENABLE;
                     GPE_GETSCOM(GPE_SCOM_ADDR_CORE(C_NET_CTRL0,
                                                    ((qloop << 2) + 3)), scom_data);
 
                     if (!(scom_data & BIT64(18)))
                     {
-                        cme_flags |= 0x4;
+                        cme_flags |= CME_CORE1_ENTRY_FIRST;
                     }
                 }
 
diff --git a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_irq_handlers.c b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_irq_handlers.c
index 85ddd82b..a1472174 100644
--- a/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_irq_handlers.c
+++ b/import/chips/p9/procedures/ppe_closed/sgpe/stop_gpe/p9_sgpe_stop_irq_handlers.c
@@ -246,16 +246,23 @@ p9_sgpe_stop_pig_handler(void* arg, PkIrqId irq)
              G_sgpe_stop_record.group.core[VECTOR_ENTRY],
              G_sgpe_stop_record.group.core[VECTOR_EXIT]);
 
-    if (G_sgpe_stop_record.group.core[VECTOR_EXIT])
+    if ((!G_sgpe_stop_record.group.core[VECTOR_EXIT]) &&
+        (!G_sgpe_stop_record.group.core[VECTOR_ENTRY]))
     {
-        PK_TRACE("unblock exit");
-        pk_semaphore_post(&(G_sgpe_stop_record.sem[1]));
+        out32(OCB_OIMR1_CLR, (BITS32(15, 2) | BIT32(19)));
     }
-
-    if (G_sgpe_stop_record.group.core[VECTOR_ENTRY])
+    else
     {
-        PK_TRACE("unblock entry");
-        pk_semaphore_post(&(G_sgpe_stop_record.sem[0]));
-    }
+        if (G_sgpe_stop_record.group.core[VECTOR_EXIT])
+        {
+            PK_TRACE("unblock exit");
+            pk_semaphore_post(&(G_sgpe_stop_record.sem[1]));
+        }
 
+        if (G_sgpe_stop_record.group.core[VECTOR_ENTRY])
+        {
+            PK_TRACE("unblock entry");
+            pk_semaphore_post(&(G_sgpe_stop_record.sem[0]));
+        }
+    }
 }