STOP: enable decrementor wakeup

EPM Note - MARKER changes

Change-Id: Idc99ab364107f25f48d74286570244f9f79b3e2e
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/39530
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Reviewed-by: Michael S. Floyd <mfloyd@us.ibm.com>
Reviewed-by: AMIT KUMAR <akumar3@us.ibm.com>
Reviewed-by: Gregory S. Still <stillgs@us.ibm.com>

11 files changed, 165 insertions, 104 deletions

diff --git a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.c b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.c
index 7dee03ad..b5aa77cd 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_irq.c
@@ -53,6 +53,7 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
     /* 0: IDX_PRTY_VEC    1: IDX_MASK_VEC */
     {
         IRQ_VEC_PRTY0_CME, /* 0: IDX_PRTY_LVL_HIPRTY */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -67,6 +68,7 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
     },
     {
         IRQ_VEC_PRTY1_CME, /* 1: IDX_PRTY_LVL_DB3 */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -80,6 +82,7 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
     },
     {
         IRQ_VEC_PRTY2_CME, /* 2: IDX_PRTY_LVL_DB2 */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -92,6 +95,7 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
     },
     {
         IRQ_VEC_PRTY3_CME, /* 3: IDX_PRTY_LVL_SPWU */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -102,7 +106,8 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
         IRQ_VEC_PRTY3_CME
     },
     {
-        IRQ_VEC_PRTY4_CME, /* 4: IDX_PRTY_LVL_WAKE */
+        IRQ_VEC_PRTY4_CME, /* 4: IDX_PRTY_LVL_RGWU */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -112,7 +117,8 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
         IRQ_VEC_PRTY4_CME
     },
     {
-        IRQ_VEC_PRTY5_CME, /* 5: IDX_PRTY_LVL_STOP */
+        IRQ_VEC_PRTY5_CME, /* 5: IDX_PRTY_LVL_PCWU */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -121,7 +127,8 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
         IRQ_VEC_PRTY5_CME
     },
     {
-        IRQ_VEC_PRTY6_CME, /* 6: IDX_PRTY_LVL_DB1 */
+        IRQ_VEC_PRTY6_CME, /* 6: IDX_PRTY_LVL_PM_ACTIVE */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -130,7 +137,8 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
     },
 
     {
-        IRQ_VEC_PRTY7_CME, /* 7: IDX_PRTY_LVL_DB0 */
+        IRQ_VEC_PRTY7_CME, /* 7: IDX_PRTY_LVL_DB1 */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME |
@@ -138,19 +146,26 @@ const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2] =
     },
 
     {
-        IRQ_VEC_PRTY8_CME, /* 8: IDX_PRTY_LVL_INTERCME_IN0 */
+        IRQ_VEC_PRTY8_CME, /* 8: IDX_PRTY_LVL_DB0 */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME |
         IRQ_VEC_PRTY8_CME
     },
     {
-        IRQ_VEC_PRTY9_CME, /* 9: IDX_PRTY_LVL_PMCR */
+        IRQ_VEC_PRTY9_CME, /* 9: IDX_PRTY_LVL_INTERCME_IN0 */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME |
         IRQ_VEC_PRTY9_CME
     },
     {
-        IRQ_VEC_PRTY10_CME, /* 10: IDX_PRTY_LVL_DISABLED */
+        IRQ_VEC_PRTY11_CME, /* 10: IDX_PRTY_LVL_PMCR */
+        IRQ_VEC_PRTY11_CME |
         IRQ_VEC_PRTY10_CME
+    },
+    {
+        IRQ_VEC_PRTY11_CME, /* 11: IDX_PRTY_LVL_DISABLED */
+        IRQ_VEC_PRTY11_CME
     }
 
 };
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 8693225f..f99653e8 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
@@ -51,16 +51,17 @@
 #define IDX_PRTY_LVL_DB3            1
 #define IDX_PRTY_LVL_DB2            2
 #define IDX_PRTY_LVL_SPWU           3
-#define IDX_PRTY_LVL_WAKE           4
-#define IDX_PRTY_LVL_STOP           5
-#define IDX_PRTY_LVL_DB1            6
-#define IDX_PRTY_LVL_DB0            7
-#define IDX_PRTY_LVL_INTERCME_IN0   8
-#define IDX_PRTY_LVL_PMCR           9
-#define IDX_PRTY_LVL_DISABLED       10
+#define IDX_PRTY_LVL_RGWU           4
+#define IDX_PRTY_LVL_PCWU           5
+#define IDX_PRTY_LVL_PM_ACTIVE      6
+#define IDX_PRTY_LVL_DB1            7
+#define IDX_PRTY_LVL_DB0            8
+#define IDX_PRTY_LVL_INTERCME_IN0   9
+#define IDX_PRTY_LVL_PMCR           10
+#define IDX_PRTY_LVL_DISABLED       11
 #define IDX_PRTY_VEC                0
 #define IDX_MASK_VEC                1
-#define NUM_EXT_IRQ_PRTY_LEVELS  (uint8_t)(11)
+#define NUM_EXT_IRQ_PRTY_LEVELS  (uint8_t)(12)
 extern const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2];
 
 // Group0: Non-task hi-prty IRQs
@@ -71,20 +72,22 @@ extern const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2];
 #define IRQ_VEC_PRTY2_CME   (uint64_t)(0x0000300000000000)
 // Group3: SPWU
 #define IRQ_VEC_PRTY3_CME   (uint64_t)(0x0003000000000000)
-// Group4: WAKE
-#define IRQ_VEC_PRTY4_CME   (uint64_t)(0x000CC00000000000)
-// Group5: STOP
-#define IRQ_VEC_PRTY5_CME   (uint64_t)(0x00000C0000000000)
-// Group6: DB1
-#define IRQ_VEC_PRTY6_CME   (uint64_t)(0x0000000000C00000)
-// Group7: DB0
-#define IRQ_VEC_PRTY7_CME   (uint64_t)(0x000000000C000000)
-// Group8: INTERCME_IN0
-#define IRQ_VEC_PRTY8_CME   (uint64_t)(0x0100000000000000)
-// Group9: PMCR
-#define IRQ_VEC_PRTY9_CME   (uint64_t)(0x0000000030000000)
-// Group10: We should never detect these
-#define IRQ_VEC_PRTY10_CME  (uint64_t)(0x00C003FFC33FFFFF)
+// Group4: RGWU
+#define IRQ_VEC_PRTY4_CME   (uint64_t)(0x0000C00000000000)
+// Group5: PCWU
+#define IRQ_VEC_PRTY5_CME   (uint64_t)(0x000C000000000000)
+// Group6: PM_ACTIVE
+#define IRQ_VEC_PRTY6_CME   (uint64_t)(0x00000C0000000000)
+// Group7: DB1
+#define IRQ_VEC_PRTY7_CME   (uint64_t)(0x0000000000C00000)
+// Group8: DB0
+#define IRQ_VEC_PRTY8_CME   (uint64_t)(0x000000000C000000)
+// Group9: INTERCME_IN0
+#define IRQ_VEC_PRTY9_CME   (uint64_t)(0x0100000000000000)
+// Group10: PMCR
+#define IRQ_VEC_PRTY10_CME  (uint64_t)(0x0000000030000000)
+// Group11: We should never detect these
+#define IRQ_VEC_PRTY11_CME  (uint64_t)(0x00C003FFC33FFFFF)
 
 // This should be 0xFFFFFFFFFFFFFFFF
 #define IRQ_VEC_PRTY_CHECK  ( IRQ_VEC_PRTY0_CME | \
@@ -97,7 +100,8 @@ extern const uint64_t ext_irq_vectors_cme[NUM_EXT_IRQ_PRTY_LEVELS][2];
                               IRQ_VEC_PRTY7_CME | \
                               IRQ_VEC_PRTY8_CME | \
                               IRQ_VEC_PRTY9_CME | \
-                              IRQ_VEC_PRTY10_CME )
+                              IRQ_VEC_PRTY10_CME | \
+                              IRQ_VEC_PRTY11_CME )
 
 extern uint8_t       g_current_prty_level;
 extern uint8_t       g_eimr_stack[NUM_EXT_IRQ_PRTY_LEVELS];
diff --git a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_main.c b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_main.c
index a0edef2e..fd03f2b2 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/p9_cme_main.c
+++ b/import/chips/p9/procedures/ppe_closed/cme/p9_cme_main.c
@@ -56,17 +56,17 @@ IRQ_HANDLER_DEFAULT                         //CMEHW_IRQ_BCE_BUSY_HIGH
 IRQ_HANDLER_DEFAULT                         //CMEHW_IRQ_BCE_TIMEOUT
 IRQ_HANDLER_DEFAULT                         //CMEHW_IRQ_DOORBELL3_C0
 IRQ_HANDLER_DEFAULT                         //CMEHW_IRQ_DOORBELL3_C1
-IRQ_HANDLER(p9_cme_stop_exit_handler, (void*) & (G_cme_stop_record.sem[1]))
+IRQ_HANDLER(p9_cme_stop_pcwu_handler, (void*) & (G_cme_stop_record.sem[1]))
 //CMEHW_IRQ_PC_INTR_PENDING_C0
-IRQ_HANDLER(p9_cme_stop_exit_handler, (void*) & (G_cme_stop_record.sem[1]))
+IRQ_HANDLER(p9_cme_stop_pcwu_handler, (void*) & (G_cme_stop_record.sem[1]))
 //CMEHW_IRQ_PC_INTR_PENDING_C1
 IRQ_HANDLER(p9_cme_stop_spwu_handler, (void*) & (G_cme_stop_record.sem[1]))
 //CMEHW_IRQ_SPECIAL_WAKEUP_C0
 IRQ_HANDLER(p9_cme_stop_spwu_handler, (void*) & (G_cme_stop_record.sem[1]))
 //CMEHW_IRQ_SPECIAL_WAKEUP_C1
-IRQ_HANDLER(p9_cme_stop_exit_handler, (void*) & (G_cme_stop_record.sem[1]))
+IRQ_HANDLER(p9_cme_stop_rgwu_handler, (void*) & (G_cme_stop_record.sem[1]))
 //CMEHW_IRQ_REG_WAKEUP_C0
-IRQ_HANDLER(p9_cme_stop_exit_handler, (void*) & (G_cme_stop_record.sem[1]))
+IRQ_HANDLER(p9_cme_stop_rgwu_handler, (void*) & (G_cme_stop_record.sem[1]))
 //CMEHW_IRQ_REG_WAKEUP_C1
 IRQ_HANDLER(p9_cme_stop_db2_handler, 0)     //CMEHW_IRQ_DOORBELL2_C0
 IRQ_HANDLER(p9_cme_stop_db2_handler, 0)     //CMEHW_IRQ_DOORBELL2_C1
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 11719101..312cccb3 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
@@ -278,6 +278,8 @@ typedef struct
     uint32_t      core_running;
     // core stop process handoff to sgpe from cme, used to mask pc_intr_pending
     uint32_t      core_stopgpe;
+    // core with pc_intr_pending blocked for decrementor stop5 wakeup support
+    uint32_t      core_blockpc;
     // core in block wakeup mode, can be used as core select in scom address or data
     uint32_t      core_blockwu;
     // core in special wakeup, can be used as core select in scom address or data
@@ -305,7 +307,8 @@ void p9_cme_stop_exit();
 int  p9_cme_stop_exit_catchup(uint32_t*, uint32_t*, uint32_t*, uint8_t*, int*);
 
 void p9_cme_stop_enter_handler(void*, PkIrqId);
-void p9_cme_stop_exit_handler(void*, PkIrqId);
+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_db1_handler(void*, PkIrqId);
 void p9_cme_stop_db2_handler(void*, PkIrqId);
diff --git a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_enter_marks.h b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_enter_marks.h
index 1fbc75be..ac7ddc07 100644
--- a/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_enter_marks.h
+++ b/import/chips/p9/procedures/ppe_closed/cme/stop_cme/p9_cme_stop_enter_marks.h
@@ -39,10 +39,11 @@ enum CME_SE_MARKS
     BEGINSCOPE_STOP_ENTRY       =   0x0,
     ENDSCOPE_STOP_ENTRY     =   0x8,
     STOP_ENTER_HANDLER      =   0x10,
-    STOP_EXIT_HANDLER      =   0x18,
-    STOP_DB1_HANDLER       =   0x20,
-    STOP_DB2_HANDLER       =   0x28,
-    STOP_SPWU_HANDLER       =   0x30,
+    STOP_PCWU_HANDLER      =   0x18,
+    STOP_SPWU_HANDLER       =   0x20,
+    STOP_RGWU_HANDLER       =   0x28,
+    STOP_DB1_HANDLER       =   0x30,
+    STOP_DB2_HANDLER       =   0x38,
     SE_QUIESCE_CORE_INTF        =   0x68,
     SE_STOP_CORE_CLKS       =   0xe0,
     SE_STOP_CORE_GRID       =   0xe8,
@@ -70,10 +71,11 @@ const std::vector<CME_SE_MARKS> MARKS =
     BEGINSCOPE_STOP_ENTRY,
     ENDSCOPE_STOP_ENTRY,
     STOP_ENTER_HANDLER,
-    STOP_EXIT_HANDLER,
+    STOP_PCWU_HANDLER,
+    STOP_SPWU_HANDLER,
+    STOP_RGWU_HANDLER,
     STOP_DB1_HANDLER,
     STOP_DB2_HANDLER,
-    STOP_SPWU_HANDLER,
     SE_QUIESCE_CORE_INTF,
     SE_STOP_CORE_CLKS,
     SE_STOP_CORE_GRID,
@@ -97,10 +99,11 @@ const std::map<CME_SE_MARKS, std::string> mMARKS = boost::assign::map_list_of
         (BEGINSCOPE_STOP_ENTRY, "BEGINSCOPE_STOP_ENTRY")
         (ENDSCOPE_STOP_ENTRY, "ENDSCOPE_STOP_ENTRY")
         (STOP_ENTER_HANDLER, "STOP_ENTER_HANDLER")
-        (STOP_EXIT_HANDLER, "STOP_EXIT_HANDLER")
+        (STOP_PCWU_HANDLER, "STOP_PCWU_HANDLER")
+        (STOP_SPWU_HANDLER, "STOP_SPWU_HANDLER")
+        (STOP_RGWU_HANDLER, "STOP_RGWU_HANDLER")
         (STOP_DB1_HANDLER, "STOP_DB1_HANDLER")
         (STOP_DB2_HANDLER, "STOP_DB2_HANDLER")
-        (STOP_SPWU_HANDLER, "STOP_SPWU_HANDLER")
         (SE_QUIESCE_CORE_INTF, "SE_QUIESCE_CORE_INTF")
         (SE_STOP_CORE_CLKS, "SE_STOP_CORE_CLKS")
         (SE_STOP_CORE_GRID, "SE_STOP_CORE_GRID")
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 2b5dfabc..87a60644 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
@@ -1253,11 +1253,13 @@ p9_cme_stop_entry()
                 {
                     CME_PUTSCOM(CPPM_CPMMR_OR, core_mask, BIT64(10));
                     pig.fields.req_intr_type = PIG_TYPE3;
+                    G_cme_stop_record.core_blockpc |= core;
                 }
                 else
                 {
                     CME_PUTSCOM(CPPM_CPMMR_CLR, core_mask, BIT64(10));
                     pig.fields.req_intr_type = PIG_TYPE2;
+                    G_cme_stop_record.core_blockpc &= ~core;
                 }
 
                 pig.fields.req_intr_payload = G_cme_stop_record.req_level[core_index];
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 5ac7d97e..a73eb505 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
@@ -233,6 +233,7 @@ void p9_cme_stop_exit_end(uint32_t core, uint32_t spwu_stop)
 
     G_cme_stop_record.core_running |=  core;
     G_cme_stop_record.core_stopgpe &= ~core;
+    G_cme_stop_record.core_blockpc &= ~core;
 
     PK_TRACE_DBG("Drop halt STOP override disable via LMCR[14/15]");
     out32(CME_LCL_LMCR_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 709489f9..5cfe3d16 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
@@ -29,6 +29,52 @@
 
 extern CmeStopRecord G_cme_stop_record;
 
+
+void
+p9_cme_stop_pcwu_handler(void* arg, PkIrqId irq)
+{
+    MARK_TRAP(STOP_PCWU_HANDLER)
+    PK_TRACE_INF("PCWU Handler Trigger %d", irq);
+
+    uint32_t  core_mask = 0;
+    uint32_t  core      = (in32(CME_LCL_EISR) & BITS32(12, 2)) >> SHIFT32(13);
+    data64_t  scom_data = {0};
+    ppm_pig_t pig       = {0};
+
+    for (core_mask = 2; core_mask; core_mask--)
+    {
+        if (core & core_mask)
+        {
+            CME_GETSCOM(CPPM_CPMMR, core_mask, CME_SCOM_AND, scom_data.value);
+
+            // If notify_select == sgpe
+            if (scom_data.words.upper & BIT32(13))
+            {
+                // In stop5 as using type2, send exit pig
+                if (!(scom_data.words.upper & BIT32(10)))
+                {
+                    pig.fields.req_intr_type    = PIG_TYPE2;
+                    pig.fields.req_intr_payload = 0x400;
+                    CME_PUTSCOM(PPM_PIG, core_mask, pig.value);
+                }
+
+                // block pc for stop8,11 or stop5 as pig sent
+                out32(CME_LCL_EIMR_OR, core_mask << SHIFT32(13));
+                G_cme_stop_record.core_blockpc |= core_mask;
+                core = core - core_mask;
+            }
+        }
+    }
+
+    // if still wakeup for core with notify_select == cme, go exit
+    if (core)
+    {
+        out32(CME_LCL_EIMR_OR, BITS32(12, 6) | BITS32(20, 2));
+        pk_semaphore_post((PkSemaphore*)arg);
+    }
+}
+
+
 // When take an Interrupt on falling edge of SPWU from a CPPM.
 // 1) Read EINR to check if another one has been set
 //    in the meantime from the same core.  If so abort.
@@ -117,11 +163,10 @@ p9_cme_stop_spwu_handler(void* arg, PkIrqId irq)
 
 
 void
-p9_cme_stop_exit_handler(void* arg, PkIrqId irq)
+p9_cme_stop_rgwu_handler(void* arg, PkIrqId irq)
 {
-    MARK_TRAP(STOP_EXIT_HANDLER)
-    PK_TRACE_INF("SX Handler");
-    PK_TRACE_DBG("SX Trigger %d", irq);
+    MARK_TRAP(STOP_RGWU_HANDLER)
+    PK_TRACE_INF("RGWU Handler Trigger %d", irq);
     out32(CME_LCL_EIMR_OR, BITS32(12, 6) | BITS32(20, 2));
     pk_semaphore_post((PkSemaphore*)arg);
 }
@@ -132,71 +177,35 @@ void
 p9_cme_stop_enter_handler(void* arg, PkIrqId irq)
 {
     MARK_TRAP(STOP_ENTER_HANDLER)
-    PK_TRACE_INF("SE Handler");
-    PK_TRACE_DBG("SE Trigger %d", irq);
+    PK_TRACE_INF("PM_ACTIVE Handler Trigger %d", irq);
     out32(CME_LCL_EIMR_OR, BITS32(12, 6) | BITS32(20, 2));
     pk_semaphore_post((PkSemaphore*)arg);
 }
 
-
-
 void
 p9_cme_stop_db1_handler(void* arg, PkIrqId irq)
 {
     PkMachineContext ctx;
     MARK_TRAP(STOP_DB1_HANDLER)
-    PK_TRACE_INF("DB1 Handler");
-    PK_TRACE_DBG("DB1 Trigger %d", irq);
+    PK_TRACE_DBG("DB1 Handler Trigger %d", irq);
     pk_irq_vec_restore(&ctx);
 }
 
-
-
 void
 p9_cme_stop_db2_handler(void* arg, PkIrqId irq)
 {
     PkMachineContext ctx;
-    cppm_cmedb2_t    db2c0 = {0};
-    cppm_cmedb2_t    db2c1 = {0};
-
     MARK_TRAP(STOP_DB2_HANDLER)
-    PK_TRACE_INF("DB2 Handler");
-    PK_TRACE_DBG("DB2 Trigger %d", irq);
-
-    CME_GETSCOM(CPPM_CMEDB2, CME_MASK_C0, CME_SCOM_AND, db2c0.value);
-    CME_GETSCOM(CPPM_CMEDB2, CME_MASK_C1, CME_SCOM_AND, db2c1.value);
-    CME_PUTSCOM(CPPM_CMEDB2, CME_MASK_BC, 0);
-    out32(CME_LCL_EISR_CLR,  BITS32(18, 2));
-
-    if (db2c0.fields.cme_message_numbern == DB2_BLOCK_WKUP_ENTRY)
-    {
-        G_cme_stop_record.core_blockwu |= CME_MASK_C0;
-        g_eimr_override                |= IRQ_VEC_PCWU_C0;
-    }
-    else if (db2c0.fields.cme_message_numbern == DB2_BLOCK_WKUP_EXIT)
-    {
-        G_cme_stop_record.core_blockwu &= ~CME_MASK_C0;
-        g_eimr_override                &= ~IRQ_VEC_PCWU_C0;
-    }
-
-    if (db2c1.fields.cme_message_numbern == DB2_BLOCK_WKUP_ENTRY)
-    {
-        G_cme_stop_record.core_blockwu |= CME_MASK_C1;
-        g_eimr_override                |= IRQ_VEC_PCWU_C1;
-    }
-    else if (db2c1.fields.cme_message_numbern == DB2_BLOCK_WKUP_EXIT)
-    {
-        G_cme_stop_record.core_blockwu &= ~CME_MASK_C1;
-        g_eimr_override                &= ~IRQ_VEC_PCWU_C1;
-    }
+    PK_TRACE_DBG("DB2 Handler Trigger %d", irq);
 
-    out32(CME_LCL_SICR_OR,  G_cme_stop_record.core_blockwu << SHIFT32(3));
-    out32(CME_LCL_SICR_CLR,
-          (~G_cme_stop_record.core_blockwu & CME_MASK_BC) << SHIFT32(3));
+    // read and clear doorbell
+    uint32_t core = (in32(CME_LCL_EISR) & BITS32(18, 2)) >> SHIFT32(19);
+    CME_PUTSCOM(CPPM_CMEDB2, core, 0);
+    G_cme_stop_record.core_blockpc &= ~core;
 
-    out32(CME_LCL_FLAGS_OR, G_cme_stop_record.core_blockwu << SHIFT32(9));
-    out32(CME_LCL_FLAGS_CLR,
-          (~G_cme_stop_record.core_blockwu & CME_MASK_BC) << SHIFT32(9));
+    // unmask pc interrupt pending to wakeup that is still pending
+    core &= (~(G_cme_stop_record.core_running));
+    out32(CME_LCL_EIMR_CLR, core << SHIFT32(13));
 
     pk_irq_vec_restore(&ctx);
 }
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 f1ee5dfd..8ff061b3 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
@@ -251,7 +251,8 @@ enum SGPE_STOP_VECTOR_INDEX
     VECTOR_EXIT                       = 0,
     VECTOR_ENTRY                      = 1,
     VECTOR_CONFIG                     = 2,
-    VECTOR_ACTIVE                     = 3
+    VECTOR_ACTIVE                     = 3,
+    VECTOR_PCWU                       = 4
 };
 
 typedef struct
@@ -270,7 +271,7 @@ typedef struct
 
 typedef struct
 {
-    uint32_t core[4]; // 24 bits
+    uint32_t core[5]; // 24 bits
     uint32_t quad[4]; // 6 bits
     uint32_t ex_l[3]; // 6 bits
     uint32_t ex_r[3]; // 6 bits
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 131ca7c5..26d6647d 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
@@ -276,6 +276,14 @@ void p9_sgpe_stop_exit_end(uint32_t cexit, uint32_t qspwu, uint32_t qloop)
 
 #endif
 
+        // if waken up by pc, send doorbell to unmask pc
+        if (G_sgpe_stop_record.group.core[VECTOR_PCWU] & BIT32(((qloop << 2) + cloop)))
+        {
+            p9_dd1_db_unicast_wr(GPE_SCOM_ADDR_CORE(CPPM_CMEDB2,
+                                                    ((qloop << 2) + cloop)), BIT64(7));
+            G_sgpe_stop_record.group.core[VECTOR_PCWU] &= ~BIT32(((qloop << 2) + cloop));
+        }
+
         PK_TRACE_INF("SX.CME: Core[%d] Switch CorePPM Wakeup Back to CME via CPMMR[13]",
                      ((qloop << 2) + cloop));
         p9_dd1_cppm_unicast_wr(
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 00f6e298..961c163a 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
@@ -46,8 +46,8 @@ SgpeStopRecord G_sgpe_stop_record __attribute__((section (".dump_ptrs"))) =
         {0, 0, 0, 0, 0, 0, 0}
     },
     // group vectors
-    {   {0, 0, 0},
-        {0, 0, 0},
+    {   {0, 0, 0, 0, 0},
+        {0, 0, 0, 0},
         {0, 0, 0},
         {0, 0, 0},
         {0, 0, 0},
@@ -272,15 +272,30 @@ p9_sgpe_stop_pig_handler(void* arg, PkIrqId irq)
             {
                 if (!(scom_data & BIT64(13)))
                 {
-                    PK_TRACE_ERR("ERROR: Received Type2 Entry PIG When Wakeup_notify_select = 0. HALT SGPE!");
-                    PK_PANIC(SGPE_PIG_TYPE2_ENTRY_WNS_CME);
+                    // wakeup=normal + notify=cme -> error
+                    if (cpayload_t2 != 0x400)
+                    {
+                        PK_TRACE_ERR("ERROR: Received Type2 Entry PIG When Wakeup_notify_select = 0. HALT SGPE!");
+                        PK_PANIC(SGPE_PIG_TYPE2_ENTRY_WNS_CME);
+                    }
+
+                    // wakeup=pc + notify=cme -> ignore phantom(already handoff to cme by other wakeup)
                 }
+                else
+                {
+                    // wakeup=pc + notify=sgpe -> go exit with flag to do extra doorbell from normal wakeup
+                    if (cpayload_t2 == 0x400)
+                    {
+                        G_sgpe_stop_record.group.core[VECTOR_PCWU] |= BIT32(((qloop << 2) + cloop));
+                    }
 
-                PK_TRACE_INF("Core Request Entry via Type2");
-                G_sgpe_stop_record.level[qloop][cloop] =
-                    (cpayload_t2 & TYPE2_PAYLOAD_STOP_LEVEL);
-                G_sgpe_stop_record.group.core[VECTOR_ENTRY] |=
-                    BIT32(((qloop << 2) + cloop));
+                    // wakeup=normal + notify=sgpe -> go exit
+                    PK_TRACE_INF("Core Request Entry via Type2");
+                    G_sgpe_stop_record.level[qloop][cloop] =
+                        (cpayload_t2 & TYPE2_PAYLOAD_STOP_LEVEL);
+                    G_sgpe_stop_record.group.core[VECTOR_ENTRY] |=
+                        BIT32(((qloop << 2) + cloop));
+                }
             }
             // if t3 entry (t2 exit or empty)
             else if (cpayload_t3 && (!(cpayload_t3 & TYPE2_PAYLOAD_EXIT_EVENT)))