PGPE: Fix FIT and actuation step conflict

    Key_Cronus_Test=PM_REGRESS

Change-Id: If59fa83c2a22a312f6bb6676b5570bae7774ee72
Original-Change-Id: I8cf51a4e044c70871c5f74a5d3a6ecfe64dafd47
CQ: SW411044
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/50779
Tested-by: PPE CI <ppe-ci+hostboot@us.ibm.com>
Tested-by: HWSV CI <hwsv-ci+hostboot@us.ibm.com>
Reviewed-by: Michael S. Floyd <mfloyd@us.ibm.com>
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Tested-by: Hostboot CI <hostboot-ci+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: Jennifer A. Stofer <stofer@us.ibm.com>

4 files changed, 302 insertions, 225 deletions

diff --git a/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.c b/import/chips/p9/procedures/ppe_closed/cme/pstate_cme/p9_cme_pstate.c
index 853a1ada..b5bf3379 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
@@ -692,6 +692,7 @@ void p9_cme_pstate_pmsr_updt(uint32_t coreMask)
     {
         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) ;
diff --git a/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_pstate.c b/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_pstate.c
index a5081bfc..752ea1bc 100644
--- a/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_pstate.c
+++ b/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_pstate.c
@@ -50,8 +50,7 @@ extern GlobalPstateParmBlock* G_gppb;
 extern uint32_t G_ext_vrm_inc_rate_mult_usperus;
 extern uint32_t G_ext_vrm_dec_rate_mult_usperus;
 extern PgpePstateRecord G_pgpe_pstate_record;
-extern void p9_pgpe_ipc_ack_sgpe_ctrl_stop_updt_core_enable(ipc_msg_t* msg, void* arg);
-extern void p9_pgpe_ipc_ack_sgpe_ctrl_stop_updt_core_disable(ipc_msg_t* msg, void* arg);
+extern void p9_pgpe_ipc_ack_sgpe_ctrl_stop_updt(ipc_msg_t* msg, void* arg);
 
 //
 //Global Data
@@ -889,6 +888,10 @@ void p9_pgpe_pstate_stop()
     G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_PSTATE_START_STOP].pending_ack = 0;
     ipc_send_rsp(G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_PSTATE_START_STOP].cmd, IPC_RC_SUCCESS);
 
+    G_pgpe_optrace_data.word[0] = (START_STOP_FLAG << 24) | (G_pgpe_pstate_record.globalPSComputed << 16)
+                                  | (in32(OCB_QCSR) >> 16);
+    p9_pgpe_optrace(PRC_START_STOP);
+
     PK_TRACE_INF("PSS: Stop Done");
 }
 
@@ -1004,7 +1007,7 @@ void p9_pgpe_pstate_process_quad_entry_done(uint32_t quadsRequested)
     PK_TRACE_INF("QE: (Done), Vec=0x%x\n", quadsRequested);
 
     //If WOF Enabled, then interlock with OCC
-    if(G_pgpe_pstate_record.wofStatus == WOF_ENABLED)
+    if(G_pgpe_pstate_record.wofStatus == WOF_ENABLED && G_pgpe_pstate_record.pstatesStatus == PSTATE_ACTIVE)
     {
         GPE_PUTSCOM(OCB_OCCFLG_OR, BIT32(REQUESTED_ACTIVE_QUAD_UPDATE));//Set OCCFLG[REQUESTED_ACTIVE_QUAD_UPDATE]
     }
@@ -1065,12 +1068,27 @@ void p9_pgpe_pstate_process_quad_exit(uint32_t quadsRequested)
 //
 // p9_pgpe_pstate_wof_ctrl
 //
-void p9_pgpe_pstate_wof_ctrl(uint32_t action, uint32_t activeCores, uint32_t activeQuads)
+void p9_pgpe_pstate_wof_ctrl(uint32_t action)
 {
     uint32_t c;
+    uint32_t activeCores, activeQuads;
 
     if (action == PGPE_ACTION_WOF_ON)
     {
+        //In WOF Phase >= 2, we ask SGPE to start sending active core updates
+        if ((G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_ENABLE_VRATIO) ||
+            (G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_VRATIO_MODIFIER))
+        {
+            p9_pgpe_pstate_send_ctrl_stop_updt(CTRL_STOP_UPDT_ENABLE_CORE);
+            activeCores = G_sgpe_control_updt.fields.active_cores << 8;
+            activeQuads = G_sgpe_control_updt.fields.active_quads;
+        }
+        else
+        {
+            activeCores = G_pgpe_pstate_record.activeDB;
+            activeQuads = G_pgpe_pstate_record.activeQuads;
+        }
+
         G_pgpe_pstate_record.wofStatus = WOF_ENABLED;
         //Set to value returned by SGPE or initial value determined during boot(equal to configured cores)
         G_pgpe_pstate_record.activeCores = activeCores << 8;
@@ -1098,17 +1116,15 @@ void p9_pgpe_pstate_wof_ctrl(uint32_t action, uint32_t activeCores, uint32_t act
     }
     else if (action == PGPE_ACTION_WOF_OFF)
     {
-        G_pgpe_pstate_record.wofStatus = WOF_DISABLED;
-        p9_pgpe_pstate_update_wof_state();
+        //In WOF Phase >= 2, we ask SGPE to stop sending active core updates
+        if ((G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_ENABLE_VRATIO) ||
+            (G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_VRATIO_MODIFIER))
+        {
+            p9_pgpe_pstate_send_ctrl_stop_updt(CTRL_STOP_UPDT_DISABLE_CORE);
+        }
 
-        //ACK back pending WOF
-        ipc_async_cmd_t* async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_CTRL].cmd;
-        ipcmsg_wof_control_t* args = (ipcmsg_wof_control_t*)async_cmd->cmd_data;
-        args->msg_cb.rc = PGPE_RC_SUCCESS;
-        G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_CTRL].pending_ack = 0;
-        ipc_send_rsp(G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_CTRL].cmd, IPC_RC_SUCCESS);
+        G_pgpe_pstate_record.wofStatus = WOF_DISABLED;
 
-        p9_pgpe_optrace(ACK_WOF_CTRL);
         PK_TRACE_DBG("WCT: WOF Disabled");
     }
 }
@@ -1125,23 +1141,12 @@ void p9_pgpe_pstate_send_ctrl_stop_updt(uint32_t action)
     G_sgpe_control_updt.fields.action = action;
     G_ipc_msg_pgpe_sgpe.cmd_data = &G_sgpe_control_updt;
 
-    if (action == CTRL_STOP_UPDT_ENABLE_CORE)
-    {
-        //Send "Enable Core Stop Updates" IPC to SGPE
-        ipc_init_msg(&G_ipc_msg_pgpe_sgpe.cmd,
-                     IPC_MSGID_PGPE_SGPE_CONTROL_STOP_UPDATES,
-                     p9_pgpe_ipc_ack_sgpe_ctrl_stop_updt_core_enable,
-                     NULL);
-        PK_TRACE_DBG("PTH: Sent CTRL_STOP_UPDT(ENABLE_CORE) to SGPE");
-    }
-    else
-    {
-        ipc_init_msg(&G_ipc_msg_pgpe_sgpe.cmd,
-                     IPC_MSGID_PGPE_SGPE_CONTROL_STOP_UPDATES,
-                     p9_pgpe_ipc_ack_sgpe_ctrl_stop_updt_core_disable,
-                     NULL);
-        PK_TRACE_DBG("PTH: Sent CTRL_STOP_UPDT(DISABLE_CORE) to SGPE");
-    }
+    //Send "Enable Core Stop Updates" IPC to SGPE
+    ipc_init_msg(&G_ipc_msg_pgpe_sgpe.cmd,
+                 IPC_MSGID_PGPE_SGPE_CONTROL_STOP_UPDATES,
+                 p9_pgpe_ipc_ack_sgpe_ctrl_stop_updt,
+                 NULL);
+    PK_TRACE_DBG("PTH: Sent CTRL_STOP_UPDT(ENABLE_CORE) to SGPE");
 
     rc = ipc_send_cmd(&G_ipc_msg_pgpe_sgpe.cmd);
 
@@ -1150,6 +1155,9 @@ void p9_pgpe_pstate_send_ctrl_stop_updt(uint32_t action)
         PK_PANIC(PGPE_SGPE_IPC_SEND_BAD_RC);
     }
 
+    //Wait for return code to be set
+    while(G_sgpe_control_updt.fields.return_code != SGPE_PGPE_IPC_RC_SUCCESS);
+
 #endif// _SGPE_IPC_ENABLED_
 }
 
@@ -1171,55 +1179,112 @@ void p9_pgpe_pstate_apply_safe_clips()
     //Update clips
     p9_pgpe_pstate_apply_clips();
 
-    PK_TRACE_INF("SCL: Apply Safe Enter");
+    PK_TRACE_INF("SCL: Apply Safe Exit");
 }
 
 //
 //p9_pgpe_pstate_safe_mode()
 //
+//Note: Must call this procedure inside sub-critical section.
+//
 void p9_pgpe_pstate_safe_mode()
 {
     PK_TRACE_INF("SAF: Safe Mode Enter");
     uint32_t occScr2 = in32(OCB_OCCS2);
     uint32_t suspend = in32(OCB_OCCFLG) & BIT32(PM_COMPLEX_SUSPEND);
-    uint32_t trace = suspend ? PRC_PM_SUSP : PRC_SAFE_MODE;
+    uint32_t safemode = in32(OCB_OCCFLG) & BIT32(PGPE_SAFE_MODE);
+
+    // Generate OPTRACE Process Start
+    G_pgpe_optrace_data.word[0] = (G_pgpe_pstate_record.activeQuads << 24) | (G_pgpe_pstate_record.globalPSComputed << 16)
+                                  | (G_pgpe_pstate_record.safePstate << 8)
+                                  | ((suspend) ? 0x2 : 0)
+                                  | ((safemode) ? 0x1 : 0);
+
+    p9_pgpe_optrace(PRC_SAFE_MODE);
 
-    if(!suspend)
+    // Apply clips and actuate to safe mode
+    p9_pgpe_pstate_apply_safe_clips();
+
+    while (p9_pgpe_pstate_at_target() == 0)
     {
-        G_pgpe_optrace_data.word[0] = (G_pgpe_pstate_record.activeQuads << 24) | (G_pgpe_pstate_record.globalPSComputed << 16)
-                                      | (G_pgpe_pstate_record.safePstate << 8) | ((in32(OCB_OCCFLG) & BIT32(PGPE_SAFE_MODE)) ? 1 : 0);
+        p9_pgpe_pstate_do_step();
     }
 
-    p9_pgpe_optrace(trace);
+    PK_TRACE_INF("SAF: Safe Mode Actuation Done!");
+
+    //Send SAFE Mode Bcast to all CMEs
+    pgpe_db0_safe_mode_bcast_t db0_sm_bcast;
+    db0_sm_bcast.value = 0;
+    db0_sm_bcast.fields.msg_id = MSGID_DB0_SAFE_MODE_BROADCAST;
+    p9_pgpe_send_db0(db0_sm_bcast.value,
+                     G_pgpe_pstate_record.activeDB,
+                     PGPE_DB0_UNICAST,
+                     PGPE_DB0_ACK_WAIT_CME,
+                     G_pgpe_pstate_record.activeQuads);
+
+    //Update PstatesStatus to PM_SUSPEND_PENDING or PSTATE_SAFE_MODE
+    G_pgpe_pstate_record.pstatesStatus = suspend ? PSTATE_PM_SUSPEND_PENDING : PSTATE_SAFE_MODE;
+
+    //Handle any pending ACKs
+    ipc_async_cmd_t* async_cmd;
 
-    if (G_pgpe_pstate_record.pstatesStatus == PSTATE_ACTIVE)
+    //ACK back to OCC with "PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE"
+    if (G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_CLIP_UPDT].pending_ack == 1)
     {
-        //In the case of suspend, if active the send_suspend is handled in actuate_pstates thread
-        p9_pgpe_pstate_apply_safe_clips();
+        async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_CLIP_UPDT].cmd;
+        ipcmsg_clip_update_t* args = (ipcmsg_clip_update_t*)async_cmd->cmd_data;
+        args->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
+        G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_CLIP_UPDT].pending_ack = 0;
+        ipc_send_rsp(G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_CLIP_UPDT].cmd, IPC_RC_SUCCESS);
+        p9_pgpe_optrace(ACK_CLIP_UPDT);
     }
-    else
+
+    //ACK back to OCC with "PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE"
+    if (G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_VFRT].pending_ack == 1)
     {
-        occScr2 |= BIT32(PGPE_SAFE_MODE_ERROR);
+        async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_VFRT].cmd;
+        ipcmsg_wof_vfrt_t* args_wof_vfrt = (ipcmsg_wof_vfrt_t*)async_cmd->cmd_data;
+        args_wof_vfrt->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
+        G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_VFRT].pending_ack = 0;
+        ipc_send_rsp(G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_WOF_VFRT].cmd, IPC_RC_SUCCESS);
+    }
 
-        if(suspend)
-        {
-            p9_pgpe_pstate_send_suspend_stop();
-        }
+    //ACK back to SGPE with "IPC_SGPE_PGPE_RC_SUCCESS"
+    if (G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_QUADS_UPDT].pending_ack == 1)
+    {
+        async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_QUADS_UPDT].cmd;
+        ipcmsg_s2p_update_active_quads_t* args = (ipcmsg_s2p_update_active_quads_t*)async_cmd->cmd_data;
+        p9_pgpe_pstate_process_quad_exit(args->fields.requested_quads << 2);
+
+        //activeQuads isn't updated until registration, so we OR with requested quads.
+        args->fields.return_active_quads = (G_pgpe_pstate_record.activeQuads >> 2) | args->fields.requested_quads;
+        args->fields.return_code = IPC_SGPE_PGPE_RC_SUCCESS;
+        G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_QUADS_UPDT].pending_ack = 0;
     }
 
-    //Update PstatesStatus
-    G_pgpe_pstate_record.pstatesStatus = suspend ? PSTATE_PM_SUSPEND_PENDING : PSTATE_SAFE_MODE;
+    //ACK back to SGPE with "IPC_SGPE_PGPE_RC_SUCCESS"
+    if(G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_CORES_UPDT].pending_ack == 1)
+    {
+        async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_CORES_UPDT].cmd;
+        ipcmsg_s2p_update_active_cores_t* args = (ipcmsg_s2p_update_active_cores_t*)async_cmd->cmd_data;
+        args->fields.return_active_cores = G_pgpe_pstate_record.activeCores >> 8;
+        args->fields.return_code = IPC_SGPE_PGPE_RC_SUCCESS;
+        G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_CORES_UPDT].pending_ack = 0;
+        ipc_send_rsp(G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_CORES_UPDT].cmd, IPC_RC_SUCCESS);
+    }
 
-    //Mark WOF Disabled so that PGPE doesn't interlock with OCC anymore
-    G_pgpe_pstate_record.wofStatus = WOF_DISABLED;
+    ///Disable WOF, so that PGPE doesn't interlock with OCC anymore
+    if(G_pgpe_pstate_record.wofStatus == WOF_ENABLED)
+    {
+        p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_OFF);
+    }
 
-    //Operation Trace Entry
-    trace = suspend ? ACK_PM_SUSP : ACK_SAFE_DONE;
-    p9_pgpe_optrace(trace);
+    //OPTRACE ACK done
+    p9_pgpe_optrace(ACK_SAFE_DONE);
 
-    //Update OCC Scratch2
+    //Update OCC Scratch2  (need to get new value because the suspend_stop callback changes the Scratch2 content)
     occScr2 &= ~BIT32(PGPE_PSTATE_PROTOCOL_ACTIVE);
-
+    occScr2 |= BIT32(PGPE_SAFE_MODE_ACTIVE);
     out32(OCB_OCCS2, occScr2);
     PK_TRACE_INF("SAF: Safe Mode Exit");
 }
@@ -1230,6 +1295,8 @@ void p9_pgpe_pstate_safe_mode()
 void p9_pgpe_pstate_send_suspend_stop()
 {
 #if SGPE_IPC_ENABLED == 1
+    p9_pgpe_optrace(PRC_PM_SUSP);
+
     int rc;
     G_sgpe_suspend_stop.fields.msg_num = MSGID_PGPE_SGPE_SUSPEND_STOP;
     G_sgpe_suspend_stop.fields.return_code = 0x0;
@@ -1287,7 +1354,7 @@ void p9_pgpe_pstate_send_suspend_stop()
 void p9_pgpe_suspend_stop_callback(ipc_msg_t* msg, void* arg)
 {
     PK_TRACE_INF("SUSP:Stop Cb");
-    p9_pgpe_optrace(SGPE_SUSP_DONE);
+    p9_pgpe_optrace(ACK_PM_SUSP);
     uint32_t occScr2 = in32(OCB_OCCS2);
     occScr2 |= BIT32(PM_COMPLEX_SUSPENDED);
     G_pgpe_pstate_record.pstatesStatus = PSTATE_PM_SUSPENDED;
diff --git a/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_actuate_pstates.c b/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_actuate_pstates.c
index 67a29571..541ee0ba 100644
--- a/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_actuate_pstates.c
+++ b/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_actuate_pstates.c
@@ -108,14 +108,15 @@ void p9_pgpe_thread_actuate_pstates(void* arg)
             //Actuate step(if needed)
             if(p9_pgpe_pstate_at_target() == 0)
             {
-                //We check that pstates are active after entering critical section
-                //It's possible that some IPC which is processed by PROCESS THREAD
-                //has updated the PstatesState to a !PSTATE_ACTIVE state
                 pk_irq_sub_critical_enter(&ctx);
 
+                //We check that pstates are active after entering critical section
+                //It's possible that some IPC or even FIT before we set actuating=1
+                //has updated the PstatesState to a !PSTATE_ACTIVE state
                 if (G_pgpe_pstate_record.pstatesStatus == PSTATE_ACTIVE)
                 {
                     p9_pgpe_pstate_do_step();
+
                 }
 
                 pk_irq_sub_critical_exit(&ctx);
@@ -130,6 +131,8 @@ void p9_pgpe_thread_actuate_pstates(void* arg)
                 //Enter sub-critical section
                 //Process any pending ACKs in sub-critical section. Otherwise, it's possible that the inrange check
                 //below is interrupted, and new clips are calculated
+                //Also, we enter sub-critical section only if some action is pending. If, move this above the
+                //if statement, then we will unnessarily enter/exit sub-critical section.
                 pk_irq_sub_critical_enter(&ctx);
 
                 inRange = 1;
@@ -184,7 +187,6 @@ void p9_pgpe_thread_actuate_pstates(void* arg)
                             G_pgpe_pstate_record.pendingPmaxClipBcast = 0;
                         }
 
-
                         ipc_async_cmd_t* async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_CLIP_UPDT].cmd;
                         ipcmsg_clip_update_t* args = (ipcmsg_clip_update_t*)async_cmd->cmd_data;
                         args->msg_cb.rc = PGPE_RC_SUCCESS;
@@ -262,39 +264,37 @@ void p9_pgpe_thread_actuate_pstates(void* arg)
                 restore_irq = 0;
                 pk_irq_vec_restore(&ctx);
             }
+        }//End PstateStatus = PSTATE_ACTIVE loop
+
+
+        //Do processing inside sub-critical section, so as to not get conflict
+        //with external interrupts
+        pk_irq_sub_critical_enter(&ctx);
+
+        PK_TRACE_DBG("ACT_TH: PSTATE_ACTIVE loop end)");
+
+        //PSTATE_SAFE_MODE_PENDING is set if, pstates are active and PM_COMPLEX_SUSPEND requested.
+        //OR, if SAFE MODE is requested
+        if (G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING)
+        {
+            p9_pgpe_pstate_safe_mode();
         }
 
-        if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING
-            || G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
+        //PSTATE_PM_SUSPEND_PENDING is directly set if PM_COMPLEX_SUSPEND is requested
+        //and pstates aren't active. Or, if PM_COMPLEX_SUSPEND is requested and pstates
+        //are active, and system has moved to Psafe(done by p9_pgpe_pstate_safe_mode() above)
+        //This check must come after the possible p9_pgpe_pstate_safe_mode() call above
+        if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING)
         {
-            //Actuate to PSAFE Pstate
-            while (p9_pgpe_pstate_at_target() == 0)
-            {
-                pk_irq_sub_critical_enter(&ctx);
-                p9_pgpe_pstate_do_step();
-                pk_irq_sub_critical_exit(&ctx);
-            }
+            p9_pgpe_pstate_send_suspend_stop();
+        }
 
-            //Send SAFE Mode Bcast to all CMEs
-            pgpe_db0_safe_mode_bcast_t db0_sm_bcast;
-            db0_sm_bcast.value = 0;
-            db0_sm_bcast.fields.msg_id = MSGID_DB0_SAFE_MODE_BROADCAST;
-            p9_pgpe_send_db0(db0_sm_bcast.value,
-                             G_pgpe_pstate_record.activeCores,
-                             PGPE_DB0_UNICAST,
-                             PGPE_DB0_ACK_WAIT_CME,
-                             G_pgpe_pstate_record.activeQuads);
-
-            if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING)
-            {
-                p9_pgpe_pstate_send_suspend_stop(); //Notify SGPE
-            }
-            else if (G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
-            {
-                uint32_t occScr2 = in32(OCB_OCCS2);
-                occScr2 |= BIT32(PGPE_SAFE_MODE_ACTIVE);
-                out32(OCB_OCCS2, occScr2);
-            }
+        if (G_pgpe_pstate_record.pstatesStatus == PSTATE_STOP_PENDING)
+        {
+            p9_pgpe_pstate_stop();
         }
+
+        pk_irq_sub_critical_exit(&ctx);
+
     }//Thread loop
 }
diff --git a/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_process_requests.c b/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_process_requests.c
index 36b9157d..4b2f7f61 100644
--- a/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_process_requests.c
+++ b/import/chips/p9/procedures/ppe_closed/pgpe/pstate_gpe/p9_pgpe_thread_process_requests.c
@@ -52,8 +52,7 @@ void p9_pgpe_process_wof_ctrl();
 void p9_pgpe_process_wof_vfrt();
 void p9_pgpe_process_set_pmcr_req();
 void p9_pgpe_process_registration();
-void p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_enable();
-void p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_disable();
+void p9_pgpe_process_ack_sgpe_ctrl_stop_updt();
 
 //
 //Process Request Thread
@@ -114,19 +113,6 @@ void p9_pgpe_thread_process_requests(void* arg)
             p9_pgpe_process_wof_ctrl();
         }
 
-        //We must process this "before" ACTIVE_CORES_UPDT  as SGPE might have sent one after
-        //sending ACK for CTRL_UPDT_STOP[Core Enable], and we must have marked wofState = Enabled
-        if (G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_ENABLE].pending_processing)
-        {
-            p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_enable();
-
-            if(G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_ENABLE].pending_ack == 1)
-            {
-                restore_irq = 0;
-            }
-        }
-
-
         if (G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_CORES_UPDT].pending_processing == 1)
         {
             p9_pgpe_process_sgpe_updt_active_cores();
@@ -137,18 +123,6 @@ void p9_pgpe_thread_process_requests(void* arg)
             }
         }
 
-        //We must process this "after" ACTIVE_CORES_UPDT to make sure any pending ACTIVE_CORES_UPDT
-        //have ACKed back to SGPE
-        if (G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_DISABLE].pending_processing)
-        {
-            p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_disable();
-
-            if(G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_DISABLE].pending_ack == 1)
-            {
-                restore_irq = 0;
-            }
-        }
-
         if (G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_QUADS_UPDT].pending_processing == 1)
         {
             p9_pgpe_process_sgpe_updt_active_quads();
@@ -190,6 +164,16 @@ void p9_pgpe_thread_process_requests(void* arg)
 
         }
 
+        if (G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_UPDT].pending_processing)
+        {
+            p9_pgpe_process_ack_sgpe_ctrl_stop_updt();
+
+            if(G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_UPDT].pending_ack == 1)
+            {
+                restore_irq = 0;
+            }
+        }
+
         if (G_pgpe_pstate_record.pendQuadsRegisterProcess != 0)
         {
             p9_pgpe_process_registration();
@@ -222,7 +206,7 @@ void p9_pgpe_thread_process_requests(void* arg)
 void p9_pgpe_process_sgpe_updt_active_cores()
 {
     PK_TRACE_DBG("PTH: Core Updt Entry");
-    uint32_t c, ack_now = 1;
+    uint32_t c, ack_now = 0;
 
     ipc_async_cmd_t* async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_PEND_SGPE_ACTIVE_CORES_UPDT].cmd;
     ipcmsg_s2p_update_active_cores_t* args = (ipcmsg_s2p_update_active_cores_t*)async_cmd->cmd_data;
@@ -235,59 +219,66 @@ void p9_pgpe_process_sgpe_updt_active_cores()
         args->fields.return_code = PGPE_WOF_RC_NOT_ENABLED;
         G_pgpe_optrace_data.word[0] = PGPE_OP_CORES_ACTIVE_IN_WOF_DISABLED;
         p9_pgpe_optrace(UNEXPECTED_ERROR);
+        ack_now = 1;
     }
     else
     {
-        //Store separately as shared SRAM location is split
-        PK_TRACE_DBG("PTH: Core Updt type=%u(0/1=EN/EX) reqCores=0x%x", args->fields.update_type, args->fields.active_cores);
-
-        //If ENTRY type then send ACK to SGPE immediately
-        //Otherwise, wait to ACK until WOF Clip has been applied(from actuate_pstate thread)
-        if (args->fields.update_type == UPDATE_ACTIVE_CORES_TYPE_ENTRY)
+        //If WOF_ENABLED=1, and pstatesStatus == ACITVE, then process active cores update
+        //Otherwise, pstatesStatus == SAFE_MODE_PENDING, and requests will be ACKed
+        //after actuating to Psafe
+        if(G_pgpe_pstate_record.pstatesStatus == PSTATE_ACTIVE)
         {
-            G_pgpe_pstate_record.activeCores &= ~(args->fields.active_cores << 8);
-            args->fields.return_active_cores = G_pgpe_pstate_record.activeCores >> 8;
-            args->fields.return_code = IPC_SGPE_PGPE_RC_SUCCESS;
-        }
-        else
-        {
-            G_pgpe_pstate_record.activeCores |= (args->fields.active_cores << 8);
-            ack_now = 0;
-        }
+            //Store separately as shared SRAM location is split
+            PK_TRACE_DBG("PTH: Core Updt type=%u(0/1=EN/EX) reqCores=0x%x", args->fields.update_type, args->fields.active_cores);
 
-        //Update Shared Memory Region
-        G_pgpe_pstate_record.pQuadState0->fields.active_cores = (G_pgpe_pstate_record.activeCores >> 16);
-        G_pgpe_pstate_record.pQuadState1->fields.active_cores = (G_pgpe_pstate_record.activeCores & 0x0000FF00);
+            //If ENTRY type then send ACK to SGPE immediately
+            //Otherwise, wait to ACK until WOF Clip has been applied(from actuate_pstate thread)
+            if (args->fields.update_type == UPDATE_ACTIVE_CORES_TYPE_ENTRY)
+            {
+                G_pgpe_pstate_record.activeCores &= ~(args->fields.active_cores << 8);
+                args->fields.return_active_cores = G_pgpe_pstate_record.activeCores >> 8;
+                args->fields.return_code = IPC_SGPE_PGPE_RC_SUCCESS;
+                ack_now = 1;
+            }
+            else
+            {
+                G_pgpe_pstate_record.activeCores |= (args->fields.active_cores << 8);
+            }
 
-        PK_TRACE_DBG("PTH: numActiveCores=0x%x,activeCores=0x%x", G_pgpe_pstate_record.numActiveCores,
-                     G_pgpe_pstate_record.activeCores);
-        PK_TRACE_DBG("PTH: quadPS2=0x%08x%08x,activeCores=0x%x", G_pgpe_pstate_record.pQuadState0->value >> 32,
-                     G_pgpe_pstate_record.pQuadState0->value, G_pgpe_pstate_record.pQuadState0->fields.active_cores );
-        PK_TRACE_DBG("PTH: quadPS1=0x%08x%08x,activeCores=0x%x", G_pgpe_pstate_record.pQuadState1->value >> 32,
-                     G_pgpe_pstate_record.pQuadState1->value, G_pgpe_pstate_record.pQuadState1->fields.active_cores);
+            //Update Shared Memory Region
+            G_pgpe_pstate_record.pQuadState0->fields.active_cores = (G_pgpe_pstate_record.activeCores >> 16);
+            G_pgpe_pstate_record.pQuadState1->fields.active_cores = (G_pgpe_pstate_record.activeCores & 0x0000FF00);
 
-        //Calculate number of active cores
-        G_pgpe_pstate_record.numActiveCores = 0;
+            PK_TRACE_DBG("PTH: numActiveCores=0x%x,activeCores=0x%x", G_pgpe_pstate_record.numActiveCores,
+                         G_pgpe_pstate_record.activeCores);
+            PK_TRACE_DBG("PTH: quadPS2=0x%08x%08x,activeCores=0x%x", G_pgpe_pstate_record.pQuadState0->value >> 32,
+                         G_pgpe_pstate_record.pQuadState0->value, G_pgpe_pstate_record.pQuadState0->fields.active_cores );
+            PK_TRACE_DBG("PTH: quadPS1=0x%08x%08x,activeCores=0x%x", G_pgpe_pstate_record.pQuadState1->value >> 32,
+                         G_pgpe_pstate_record.pQuadState1->value, G_pgpe_pstate_record.pQuadState1->fields.active_cores);
 
-        for (c = 0; c < MAX_CORES; c++)
-        {
-            if (G_pgpe_pstate_record.activeCores  & CORE_MASK(c))
+            //Calculate number of active cores
+            G_pgpe_pstate_record.numActiveCores = 0;
+
+            for (c = 0; c < MAX_CORES; c++)
             {
-                G_pgpe_pstate_record.numActiveCores++;
+                if (G_pgpe_pstate_record.activeCores  & CORE_MASK(c))
+                {
+                    G_pgpe_pstate_record.numActiveCores++;
+                }
             }
-        }
 
-        PK_TRACE_DBG("PTH: numActiveCores=0x%x", G_pgpe_pstate_record.numActiveCores);
+            PK_TRACE_DBG("PTH: numActiveCores=0x%x", G_pgpe_pstate_record.numActiveCores);
 
-        //OP_TRACE(Do before auction and wof calculation)
-        G_pgpe_optrace_data.word[0] = (G_pgpe_pstate_record.activeQuads << 24)  |
-                                      ((args->fields.update_type == UPDATE_ACTIVE_CORES_TYPE_ENTRY) ? 0x2000000 : 0x1000000) |
-                                      (G_pgpe_pstate_record.activeCores >> 8);
-        p9_pgpe_optrace(PRC_CORES_ACTV);
+            //OP_TRACE(Do before auction and wof calculation)
+            G_pgpe_optrace_data.word[0] = (G_pgpe_pstate_record.activeQuads << 24)  |
+                                          ((args->fields.update_type == UPDATE_ACTIVE_CORES_TYPE_ENTRY) ? 0x2000000 : 0x1000000) |
+                                          (G_pgpe_pstate_record.activeCores >> 8);
+            p9_pgpe_optrace(PRC_CORES_ACTV);
 
-        //Do auction and wof calculation
-        p9_pgpe_pstate_do_auction();
-        p9_pgpe_pstate_calc_wof();
+            //Do auction and wof calculation
+            p9_pgpe_pstate_do_auction();
+            p9_pgpe_pstate_calc_wof();
+        }
     }
 
     if (ack_now == 1)
@@ -345,7 +336,9 @@ void p9_pgpe_process_sgpe_updt_active_quads()
         G_pgpe_pstate_record.pReqActQuads->fields.requested_active_quads |= (args->fields.requested_quads << 2);
 
         //WOF Enabled
-        if(G_pgpe_pstate_record.wofStatus == WOF_ENABLED)
+        //If WOF_ENABLED=1, and pstatesStatus == ACITVE, then request for WOF_VFRT
+        //Otherwise, we don't as SAFE_MODE or PM_COMPLEX_SUSPEND or STOP is pending
+        if(G_pgpe_pstate_record.wofStatus == WOF_ENABLED && G_pgpe_pstate_record.pstatesStatus == PSTATE_ACTIVE)
         {
             PK_TRACE_DBG("PTH: OCCLFG[30] set");
             out32(OCB_OCCFLG_OR, BIT32(REQUESTED_ACTIVE_QUAD_UPDATE));//Set OCCFLG[REQUESTED_ACTIVE_QUAD_UPDATE]
@@ -354,7 +347,8 @@ void p9_pgpe_process_sgpe_updt_active_quads()
         else
         {
             p9_pgpe_pstate_process_quad_exit(args->fields.requested_quads << 2);
-            args->fields.return_active_quads = (G_pgpe_pstate_record.activeQuads >> 2) | args->fields.requested_quads;
+            args->fields.return_active_quads = (G_pgpe_pstate_record.activeQuads >> 2) |
+                                               args->fields.requested_quads; //activeQuads isn't updated until registration, so we OR with requested quads.
             args->fields.return_code = IPC_SGPE_PGPE_RC_SUCCESS;
         }
     }
@@ -393,11 +387,37 @@ void p9_pgpe_process_start_stop()
     }
     else if(G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING ||
             G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPENDED ||
+            G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
             G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
     {
         PK_TRACE_DBG("START_STOP: PM_SUSP/Safe");
         args->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
-        G_pgpe_optrace_data.word[0] = PGPE_OP_PSTATE_START_IN_PM_SUSP;
+
+        if(G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
+           G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
+        {
+
+            if (args->action == PGPE_ACTION_PSTATE_START)
+            {
+                G_pgpe_optrace_data.word[0] = PGPE_OP_PSTATE_START_IN_SAFE_MODE;
+            }
+            else
+            {
+                G_pgpe_optrace_data.word[0] = PGPE_OP_PSTATE_STOP_IN_SAFE_MODE;
+            }
+        }
+        else
+        {
+            if (args->action == PGPE_ACTION_PSTATE_START)
+            {
+                G_pgpe_optrace_data.word[0] = PGPE_OP_PSTATE_START_IN_PM_SUSP;
+            }
+            else
+            {
+                G_pgpe_optrace_data.word[0] = PGPE_OP_PSTATE_STOP_IN_PM_SUSP;
+            }
+        }
+
         p9_pgpe_optrace(UNEXPECTED_ERROR);
     }
     else
@@ -490,11 +510,22 @@ void p9_pgpe_process_clip_updt()
     }
     else if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPENDED ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING ||
+             G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
     {
         PK_TRACE_DBG("PTH: Clip Updt PMSUSP/Safe");
         args->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
-        G_pgpe_optrace_data.word[0] = PGPE_OP_CLIP_UPDT_IN_PM_SUSP;
+
+        if(G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
+           G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_CLIP_UPDT_IN_SAFE_MODE;
+        }
+        else
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_CLIP_UPDT_IN_PM_SUSP;
+        }
+
         p9_pgpe_optrace(UNEXPECTED_ERROR);
     }
     else
@@ -583,11 +614,22 @@ void p9_pgpe_process_wof_ctrl()
     }
     else if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPENDED ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING ||
+             G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
     {
         PK_TRACE_DBG("PTH: WOF Ctrl PMSUSP/Safe");
         args->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
-        G_pgpe_optrace_data.word[0] = PGPE_OP_WOF_CTRL_IN_PM_SUSP;
+
+        if(G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
+           G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_WOF_CTRL_IN_SAFE_MODE;
+        }
+        else
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_WOF_CTRL_IN_PM_SUSP;
+        }
+
         p9_pgpe_optrace(UNEXPECTED_ERROR);
     }
     else if(G_pgpe_pstate_record.pstatesStatus == PSTATE_INIT || G_pgpe_pstate_record.pstatesStatus == PSTATE_STOPPED)
@@ -607,17 +649,9 @@ void p9_pgpe_process_wof_ctrl()
             {
                 PK_TRACE_DBG("PTH: WOF Ctrl=ON,WOF_Enabled=0");
 
-                if ((G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_ENABLE_VRATIO) ||
-                    (G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_VRATIO_MODIFIER))
-                {
-                    p9_pgpe_pstate_send_ctrl_stop_updt(CTRL_STOP_UPDT_ENABLE_CORE);
-                }
-                else
-                {
-                    p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_ON, G_pgpe_pstate_record.activeDB >> 8, G_pgpe_pstate_record.activeQuads);
-                }
+                p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_ON);
+                ack_now = 0; //For WOF_ENABLE, we ACK after WOF_CLIP has been honored(actuate thread)
 
-                ack_now = 0;
                 G_pgpe_optrace_data.word[0] = (G_pgpe_pstate_record.activeQuads << 24) |
                                               (args->action << 16) |
                                               (in32(OCB_QCSR) >> 16);
@@ -637,21 +671,13 @@ void p9_pgpe_process_wof_ctrl()
             {
                 PK_TRACE_DBG("PTH: WOF Ctrl=OFF,WOF_Enabled=1");
 
-                if ((G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_ENABLE_VRATIO) ||
-                    (G_pgpe_header_data->g_pgpe_qm_flags & PGPE_FLAG_VRATIO_MODIFIER))
-                {
-                    p9_pgpe_pstate_send_ctrl_stop_updt(CTRL_STOP_UPDT_DISABLE_CORE);
-                }
-                else
-                {
-                    p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_OFF, G_pgpe_pstate_record.activeDB >> 8, G_pgpe_pstate_record.activeQuads);
-                }
+                //Disable WOF, and we ACK to OCC below
+                p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_OFF);
 
                 G_pgpe_optrace_data.word[0] = (G_pgpe_pstate_record.activeQuads << 24) |
                                               (args->action << 16) |
                                               (in32(OCB_QCSR) >> 16);
                 p9_pgpe_optrace(PRC_WOF_CTRL);
-                ack_now = 0;
             }
             else
             {
@@ -697,11 +723,22 @@ void p9_pgpe_process_wof_vfrt()
     }
     else if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPENDED ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING ||
+             G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
     {
         PK_TRACE_DBG("PTH: WOF VFRT PMSUSP/Safe");
         args->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
-        G_pgpe_optrace_data.word[0] = PGPE_OP_WOF_VFRT_IN_SAFE_MODE;
+
+        if(G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
+           G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_WOF_VFRT_IN_SAFE_MODE;
+        }
+        else
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_WOF_VFRT_IN_PM_SUSP;
+        }
+
         p9_pgpe_optrace(UNEXPECTED_ERROR);
     }
     else if(G_pgpe_pstate_record.pstatesStatus == PSTATE_STOPPED || G_pgpe_pstate_record.pstatesStatus == PSTATE_INIT)
@@ -798,11 +835,22 @@ void p9_pgpe_process_set_pmcr_req()
     }
     else if (G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPENDED ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_PM_SUSPEND_PENDING ||
+             G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
              G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
     {
         PK_TRACE_DBG("PTH: Set PMCR in PM_Susp/Safe");
         args->msg_cb.rc = PGPE_RC_PM_COMPLEX_SUSPEND_SAFE_MODE;
-        G_pgpe_optrace_data.word[0] = PGPE_OP_SET_PMCR_IN_PM_SUSP;
+
+        if(G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE_PENDING ||
+           G_pgpe_pstate_record.pstatesStatus == PSTATE_SAFE_MODE)
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_SET_PMCR_IN_SAFE_MODE;
+        }
+        else
+        {
+            G_pgpe_optrace_data.word[0] = PGPE_OP_SET_PMCR_IN_PM_SUSP;
+        }
+
         p9_pgpe_optrace(UNEXPECTED_ERROR);
     }
     else
@@ -1058,47 +1106,8 @@ void p9_pgpe_process_registration()
     PK_TRACE_DBG("PTH: Register Exit");
 }
 
-//
-//p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_enable
-//
-void p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_enable()
-{
-    ipc_async_cmd_t* async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_ENABLE].cmd;
-    ipcmsg_p2s_ctrl_stop_updates_t* args = (ipcmsg_p2s_ctrl_stop_updates_t*)async_cmd->cmd_data;
-    G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_ENABLE].pending_processing = 0;
-    G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_ENABLE].pending_ack = 0;
-
-    PK_TRACE_DBG("IPC: SGPE ACKed CTRL_STOP_UPDT(ENABLE_CORE)");
-
-    if (args->fields.return_code == IPC_SGPE_PGPE_RC_SUCCESS)
-    {
-        p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_ON, args->fields.active_cores, args->fields.active_quads);
-    }
-    else
-    {
-        PK_PANIC(PGPE_SGPE_CTRL_STOP_UPDT_BAD_ACK);
-    }
-}
-
-//
-//p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_disable
-//
-void p9_pgpe_process_ack_sgpe_ctrl_stop_updt_core_disable()
+void p9_pgpe_process_ack_sgpe_ctrl_stop_updt()
 {
-    ipc_async_cmd_t* async_cmd = (ipc_async_cmd_t*)G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_DISABLE].cmd;
-    ipcmsg_p2s_ctrl_stop_updates_t* args = (ipcmsg_p2s_ctrl_stop_updates_t*)async_cmd->cmd_data;
-    G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_DISABLE].pending_processing = 0;
-    G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_CORE_DISABLE].pending_ack = 0;
-
-    PK_TRACE_DBG("IPC: SGPE ACKed CTRL_STOP_UPDT(DISABLE_CORE)");
-
-    if (args->fields.return_code == SGPE_PGPE_IPC_RC_SUCCESS)
-    {
-        p9_pgpe_pstate_wof_ctrl(PGPE_ACTION_WOF_OFF, args->fields.active_cores, args->fields.active_quads);
-
-    }
-    else
-    {
-        PK_PANIC(PGPE_SGPE_CTRL_STOP_UPDT_BAD_ACK);
-    }
+    G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_UPDT].pending_processing = 0;
+    G_pgpe_pstate_record.ipcPendTbl[IPC_ACK_CTRL_STOP_UPDT].pending_ack = 0;
 }