MPIPL Start Chipops and Mpipl istep implementation

Change-Id: If0579dfdfb10eb42bc837107e38361512a416b03
RTC: 123696
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/30367
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Tested-by: FSP CI Jenkins <fsp-CI-jenkins+hostboot@us.ibm.com>
Reviewed-by: Shakeeb A. Pasha B K <shakeebbk@in.ibm.com>
Reviewed-by: Sachin Gupta <sgupta2m@in.ibm.com>

7 files changed, 467 insertions, 50 deletions

diff --git a/src/sbefw/sbecmdcntlinst.C b/src/sbefw/sbecmdcntlinst.C
index 41e1f0ba..22798693 100644
--- a/src/sbefw/sbecmdcntlinst.C
+++ b/src/sbefw/sbecmdcntlinst.C
@@ -40,9 +40,6 @@
 
 using namespace fapi2;
 
-// This is used to find out the array index in g_control_reg_map in
-// p9_thread_control.C
-static const uint8_t SINGLE_THREAD_BIT_MASK = 0x08;
 // TODO via RTC 152424
 // Currently all proecdures in core directory are in seeprom.
 // So we have to use function pointer to force a long call.
diff --git a/src/sbefw/sbecmdcntlinst.H b/src/sbefw/sbecmdcntlinst.H
index 80a985a0..3ca70b52 100644
--- a/src/sbefw/sbecmdcntlinst.H
+++ b/src/sbefw/sbecmdcntlinst.H
@@ -34,6 +34,11 @@
 
 #include <stdint.h>
 
+
+// This is used to find out the array index in g_control_reg_map in
+// p9_thread_control.C
+static const uint8_t SINGLE_THREAD_BIT_MASK = 0x08;
+
 /**
   * @brief sbeCntlInst : Implements SBE Control instructions ChipOp
   *
diff --git a/src/sbefw/sbecmdiplcontrol.C b/src/sbefw/sbecmdiplcontrol.C
index 43436512..763f86a9 100644
--- a/src/sbefw/sbecmdiplcontrol.C
+++ b/src/sbefw/sbecmdiplcontrol.C
@@ -37,8 +37,10 @@
 #include "assert.h"
 #include "sberegaccess.H"
 #include "sbestates.H"
+#include "sbecmdcntrldmt.H"
 
 #include "fapi2.H"
+#include "p9_misc_scom_addresses_fld.H"
 // Pervasive HWP Header Files ( istep 2)
 #include <p9_sbe_attr_setup.H>
 #include <p9_sbe_tp_chiplet_init1.H>
@@ -88,11 +90,22 @@
 // Nest frequency array
 #include "p9_frequency_buckets.H"
 
+// istep mpipl header files
+#include "p9_block_wakeup_intr.H"
+#include "p9_query_core_access_state.H"
+#include "p9_sbe_check_quiesce.H"
+#include "p9_l2_flush.H"
+#include "p9_l3_flush.H"
+#include "p9_sbe_sequence_drtm.H"
+#include "p9_thread_control.H"
+#include "sbecmdcntlinst.H"
+#include "p9_quad_power_off.H"
+
 #include "sbeXipUtils.H" // For getting hbbl offset
 #include "sbeutil.H" // For getting SBE_TO_NEST_FREQ_FACTOR
 // Forward declaration
 using namespace fapi2;
-ReturnCode sbeExecuteIstep (uint8_t i_major, uint8_t i_minor);
+
 bool validateIstep (uint8_t i_major, uint8_t i_minor);
 
 //typedefs
@@ -105,12 +118,40 @@ typedef ReturnCode (*sbeIstepHwpEq_t)
 typedef ReturnCode (*sbeIstepHwpCore_t)
                     (const Target<TARGET_TYPE_CORE> & i_target);
 
+typedef ReturnCode (*sbeIstepHwpExL2Flush_t)
+                    (const Target<TARGET_TYPE_EX> & i_target,
+                     const p9core::purgeData_t & i_purgeData);
+
+typedef ReturnCode (*sbeIstepHwpExL3Flush_t)
+                    (const Target<TARGET_TYPE_EX> & i_target,
+                     const uint32_t i_purgeType,
+                     const uint32_t i_purgeAddr);
+
+typedef ReturnCode (*sbeIstepHwpCoreBlockIntr_t)
+                    (const Target<TARGET_TYPE_CORE> & i_target,
+                     const p9pmblockwkup::OP_TYPE i_oper);
+
+typedef ReturnCode (*sbeIstepHwpCoreScomState_t)
+                    (const Target<TARGET_TYPE_CORE> & i_target,
+                     bool & o_isScom,
+                     bool & o_isScan);
+
+typedef ReturnCode (*sbeIstepHwpSequenceDrtm_t)
+                    (const Target<TARGET_TYPE_PROC_CHIP> & i_target,
+                     uint8_t & o_status);
+
 typedef union
 {
     sbeIstepHwpProc_t procHwp;
     sbeIstepHwpEq_t eqHwp;
     sbeIstepHwpCore_t coreHwp;
+    sbeIstepHwpExL2Flush_t exL2Hwp;
+    sbeIstepHwpExL3Flush_t exL3Hwp;
+    sbeIstepHwpCoreBlockIntr_t coreBlockIntrHwp;
+    sbeIstepHwpCoreScomState_t coreScomStateHwp;
+    sbeIstepHwpSequenceDrtm_t  procSequenceDrtm;
 }sbeIstepHwp_t;
+
 // Wrapper function for HWP IPl functions
 typedef ReturnCode (*sbeIstep_t)( sbeIstepHwp_t );
 
@@ -128,9 +169,24 @@ ReturnCode istepWithCoreConditional( sbeIstepHwp_t i_hwp);
 ReturnCode istepWithEqConditional( sbeIstepHwp_t i_hwp);
 ReturnCode istepNestFreq( sbeIstepHwp_t i_hwp);
 
+//MPIPL Specific
+ReturnCode istepWithCoreSetBlock( sbeIstepHwp_t i_hwp );
+ReturnCode istepWithCoreState( sbeIstepHwp_t i_hwp );
+ReturnCode istepMpiplRstClrTpmBits( sbeIstepHwp_t i_hwp );
+ReturnCode istepWithProcQuiesceLQASet( sbeIstepHwp_t i_hwp );
+ReturnCode istepWithExL2Flush( sbeIstepHwp_t i_hwp );
+ReturnCode istepWithExL3Flush( sbeIstepHwp_t i_hwp );
+ReturnCode istepNoOpStartMpipl( sbeIstepHwp_t i_hwp );
+ReturnCode istepWithProcSequenceDrtm( sbeIstepHwp_t i_hwp );
+ReturnCode istepMpiplSetFunctionalState( sbeIstepHwp_t i_hwp );
+ReturnCode istepMpiplSetMPIPLMode( sbeIstepHwp_t i_hwp );
+ReturnCode istepMpiplQuadPoweroff( sbeIstepHwp_t i_hwp );
+
 #ifdef SEEPROM_IMAGE
 // Using function pointer to force long call.
 p9_sbe_select_ex_FP_t p9_sbe_select_ex_hwp = &p9_sbe_select_ex;
+//p9_thread_control_FP_t threadCntlhwp = &p9_thread_control;
+extern p9_thread_control_FP_t threadCntlhwp;
 #endif
 
 //structure for mapping SBE wrapper and HWP functions
@@ -140,27 +196,16 @@ typedef struct
     sbeIstepHwp_t istepHwp;
 }istepMap_t;
 
-// Major isteps which are supported
-typedef enum
-{
-    SBE_ISTEP2 = 2,
-    SBE_ISTEP_FIRST = SBE_ISTEP2,
-    SBE_ISTEP3 = 3,
-    SBE_ISTEP_LAST_SLAVE = SBE_ISTEP3,
-    SBE_ISTEP4 = 4,
-    SBE_ISTEP5 = 5,
-    SBE_ISTEP_LAST_MASTER = SBE_ISTEP5,
-}sbe_supported_steps_t;
 
 // constants
-const uint32_t ISTEP2_MAX_SUBSTEPS = 17;
-const uint32_t ISTEP3_MAX_SUBSTEPS = 22;
-const uint32_t ISTEP4_MAX_SUBSTEPS = 34;
-const uint32_t ISTEP5_MAX_SUBSTEPS = 2;
-static const uint8_t ISTEP_MINOR_START = 1;
-static const uint8_t SLAVE_LAST_MINOR_ISTEP = 20;
-static const uint8_t ISTEP2_MINOR_START = 2;
 static const uint32_t SBE_ROLE_MASK = 0x00000002;
+static const uint32_t SBE_SYSTEM_QUIESCE_TIMEOUT_LOOP = 100;
+
+static const uint64_t SBE_LQA_DELAY_HW_US = 1000000ULL; // 1ms
+static const uint64_t SBE_LQA_DELAY_SIM_CYCLES = 0x1ULL;
+
+// Bit-33 used to checkstop the system
+static const uint64_t  N3_FIR_SYSTEM_CHECKSTOP_BIT = 33;
 
 // Globals
 // TODO: via RTC 123602 This global needs to move to a class that will store the
@@ -169,8 +214,43 @@ fapi2::ReturnCode g_iplFailRc = FAPI2_RC_SUCCESS;
 
 sbeRole g_sbeRole = SBE_ROLE_MASTER;
 
-// File static data
+static istepMap_t g_istepMpiplStartPtrTbl[MPIPL_START_MAX_SUBSTEPS] =
+        {
+#ifdef SEEPROM_IMAGE
+            // Place holder for StartMpipl Chip-op, State Change
+            { &istepNoOpStartMpipl, NULL },
+            // Find all the child cores within proc and call set block intr
+            { &istepWithCoreSetBlock, { .coreBlockIntrHwp = &p9_block_wakeup_intr }},
+            // Find all the child cores within proc and call hwp to know the
+            // scom state and call instruction control
+            { &istepWithCoreState, { .coreScomStateHwp = &p9_query_core_access_state }},
+            //  Reset the TPM and clear the TPM deconfig bit, it's not a
+            //  procedure but local SBE function
+            { &istepMpiplRstClrTpmBits, NULL },
+            // quiesce state for all units on the powerbus on its chip
+            { &istepWithProcQuiesceLQASet, { .procHwp = &p9_sbe_check_quiesce }},
+            // L2 cache flush via purge engine on each EX
+            { &istepWithExL2Flush, { .exL2Hwp = &p9_l2_flush }},
+            // L3 cache flush via purge engine on each EX
+            { &istepWithExL3Flush, { .exL3Hwp = &p9_l3_flush }},
+            // Check on Quiescing of all Chips in a System by Local SBE
+            { &istepWithProcSequenceDrtm, { .procSequenceDrtm = &p9_sbe_sequence_drtm }},
+#endif
+        };
+
+static istepMap_t g_istepMpiplContinuePtrTbl[MPIPL_CONTINUE_MAX_SUBSTEPS] =
+        {
+#ifdef SEEPROM_IMAGE
+            // Setup EC/EQ guard records
+            { NULL, NULL},
+            // place holder for p9_quad_power_off
+            { istepMpiplQuadPoweroff, { .eqHwp = &p9_quad_power_off} },
+            // Set MPIPL mode in Sratch Reg 3
+            { NULL, NULL},
+#endif
+        };
 
+// File static data
 static istepMap_t g_istep2PtrTbl[ ISTEP2_MAX_SUBSTEPS ] =
          {
 #ifdef SEEPROM_IMAGE
@@ -380,6 +460,16 @@ ReturnCode sbeExecuteIstep (const uint8_t i_major, const uint8_t i_minor)
                               g_istep5PtrTbl[i_minor-1].istepHwp);
             break;
 
+        case SBE_ISTEP_MPIPL_START:
+            rc = (g_istepMpiplStartPtrTbl[i_minor-1].istepWrapper)(
+                        g_istepMpiplStartPtrTbl[i_minor-1].istepHwp);
+            break;
+
+        case SBE_ISTEP_MPIPL_CONTINUE:
+            rc = (g_istepMpiplContinuePtrTbl[i_minor-1].istepWrapper)(
+                        g_istepMpiplContinuePtrTbl[i_minor-1].istepHwp);
+            break;
+
         // We should never reach here as before calling this validation has
         // been done.
         default:
@@ -416,17 +506,6 @@ bool validateIstep (const uint8_t i_major, const uint8_t i_minor)
             break;
         }
 
-        if((SBE_ROLE_SLAVE == g_sbeRole) &&
-           ((SBE_ISTEP_LAST_SLAVE < i_major) ||
-            ((SBE_ISTEP_LAST_SLAVE == i_major) &&
-             (SLAVE_LAST_MINOR_ISTEP < i_minor)
-            )))
-        {
-            // Cannot run beyond 3.20 on a slave SBE
-            valid = false;
-            break;
-        }
-
         switch( i_major )
         {
             case SBE_ISTEP2:
@@ -439,25 +518,46 @@ bool validateIstep (const uint8_t i_major, const uint8_t i_minor)
                 break;
 
             case SBE_ISTEP3:
-                if( i_minor > ISTEP3_MAX_SUBSTEPS ) { valid = false; } ;
+                if( (i_minor > ISTEP3_MAX_SUBSTEPS ) ||
+                    ((SBE_ROLE_SLAVE == g_sbeRole) &&
+                    (i_minor > SLAVE_LAST_MINOR_ISTEP)) )
+                {
+                    valid = false;
+                }
                 break;
 
             case SBE_ISTEP4:
-                if( i_minor > ISTEP4_MAX_SUBSTEPS )
+                if( (i_minor > ISTEP4_MAX_SUBSTEPS ) ||
+                    (SBE_ROLE_SLAVE == g_sbeRole) )
                 {
                     valid = false;
                 }
                 break;
 
             case SBE_ISTEP5:
-                if( i_minor > ISTEP5_MAX_SUBSTEPS )
+                if( (i_minor > ISTEP5_MAX_SUBSTEPS ) ||
+                    (SBE_ROLE_SLAVE == g_sbeRole) )
+                {
+                    valid = false;
+                }
+                break;
+
+            case SBE_ISTEP_MPIPL_START:
+                if( i_minor > MPIPL_START_MAX_SUBSTEPS )
+                {
+                    valid = false;
+                }
+                break;
+
+            case SBE_ISTEP_MPIPL_CONTINUE:
+                if( i_minor > MPIPL_CONTINUE_MAX_SUBSTEPS )
                 {
                     valid = false;
                 }
                 break;
 
             default:
-                valid= false;
+                valid = false;
                 break;
         }
     } while(0);
@@ -754,4 +854,263 @@ void sbeDoContinuousIpl()
     #undef SBE_FUNC
 }
 
+// MPIPL Specific
+//----------------------------------------------------------------------------
+ReturnCode istepWithCoreSetBlock( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepWithCoreSetBlock"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+    Target<TARGET_TYPE_PROC_CHIP > l_procTgt = plat_getChipTarget();
+    for (auto l_coreTgt : l_procTgt.getChildren<fapi2::TARGET_TYPE_CORE>())
+    {
+        SBE_EXEC_HWP(l_rc, i_hwp.coreBlockIntrHwp, l_coreTgt, p9pmblockwkup::SET)
+        if(l_rc != FAPI2_RC_SUCCESS)
+        {
+            SBE_ERROR(SBE_FUNC " p9_block_wakeup_intr failed, RC=[0x%08X]",
+                l_rc);
+            break;
+        }
+    }
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepWithCoreState( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepWithCoreState"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+    Target<TARGET_TYPE_PROC_CHIP > l_procTgt = plat_getChipTarget();
+    for (auto l_coreTgt : l_procTgt.getChildren<fapi2::TARGET_TYPE_CORE>())
+    {
+        bool l_isScanEnable = false;
+        bool l_isCoreScomEnabled = false;
+        SBE_EXEC_HWP(l_rc, i_hwp.coreScomStateHwp, l_coreTgt,
+                     l_isCoreScomEnabled, l_isScanEnable)
+        if(l_rc != FAPI2_RC_SUCCESS)
+        {
+            SBE_ERROR(SBE_FUNC " p9_query_core_access_state failed, "
+               "RC=[0x%08X]", l_rc);
+            break;
+        }
+        if(l_isCoreScomEnabled) //true
+        {
+            uint8_t l_thread = SMT4_THREAD0;
+            fapi2::buffer<uint64_t> l_data64;
+            uint64_t l_state;
+            bool l_warnCheck = true;
+            do
+            {
+                // Call instruction control stop
+                // TODO RTC 164425 - Can we pass in 1111 i.e. all threads at the
+                // same time instead of individual threads
+                SBE_EXEC_HWP(l_rc, threadCntlhwp, l_coreTgt,
+                             (SINGLE_THREAD_BIT_MASK >> l_thread),
+                             PTC_CMD_STOP, l_warnCheck,l_data64, l_state)
+                if(l_rc != FAPI2_RC_SUCCESS)
+                {
+                    SBE_ERROR(SBE_FUNC "p9_thread_control stop Failed for "
+                        "Core Thread  RC[0x%08X]", l_rc);
+                    break;
+                }
+            }while(++l_thread < SMT4_THREAD_MAX);
+        }
+    }
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepMpiplRstClrTpmBits( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepMpiplRstClrTpmBits"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+
+    // TODO RTC 150711 - Security milestone
+
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepWithExL2Flush( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepWithExL2Flush"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+
+    Target<TARGET_TYPE_PROC_CHIP > l_procTgt = plat_getChipTarget();
+    for (auto l_exTgt : l_procTgt.getChildren<fapi2::TARGET_TYPE_EX>())
+    {
+        p9core::purgeData_t l_purgeData;
+        // TODO RTC 164425 need to check if L2 is Scomable
+        // This will come from the HWP team.
+        SBE_EXEC_HWP(l_rc, i_hwp.exL2Hwp, l_exTgt, l_purgeData)
+        if(l_rc != FAPI2_RC_SUCCESS)
+        {
+            SBE_ERROR(SBE_FUNC " p9_l2_flush failed, RC=[0x%08X]", l_rc);
+            break;
+        }
+    }
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepWithExL3Flush( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepWithExL3Flush"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+
+    Target<TARGET_TYPE_PROC_CHIP > l_procTgt = plat_getChipTarget();
+    for (auto l_exTgt : l_procTgt.getChildren<fapi2::TARGET_TYPE_EX>())
+    {
+        // TODO RTC 164425 need to check if L3 is Scomable
+        // This will come from the HWP team.
+        SBE_EXEC_HWP(l_rc, i_hwp.exL3Hwp, l_exTgt, L3_FULL_PURGE, 0x0)
+        if(l_rc != FAPI2_RC_SUCCESS)
+        {
+            SBE_ERROR(SBE_FUNC " p9_l3_flush failed, RC=[0x%08X]", l_rc);
+            break;
+        }
+    }
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepWithProcSequenceDrtm( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepWithProcSequenceDrtm"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+    Target<TARGET_TYPE_PROC_CHIP > l_procTgt = plat_getChipTarget();
+
+    uint8_t l_status = 0;
+    size_t l_timeOut = SBE_SYSTEM_QUIESCE_TIMEOUT_LOOP;
+    while(l_timeOut)
+    {
+        SBE_EXEC_HWP(l_rc, i_hwp.procSequenceDrtm, l_procTgt, l_status)
+        if(l_rc != FAPI2_RC_SUCCESS)
+        {
+            SBE_ERROR(SBE_FUNC "p9_sbe_sequence_drtm failed, RC=[0x%08X]",l_rc);
+            break;
+        }
+        if(l_status)
+        {
+            SBE_INFO(SBE_FUNC "p9_sbe_sequence_drtm LQA SBE System Quiesce done");
+            break;
+        }
+        else
+        {
+            l_timeOut--;
+            // delay prior to repeating the above
+            FAPI_TRY(fapi2::delay(SBE_LQA_DELAY_HW_US, SBE_LQA_DELAY_SIM_CYCLES),
+                     "Error from delay");
+        }
+    }
+    // Checkstop system if SBE system quiesce not set after the loop
+    if(!l_status || l_rc)
+    {
+        SBE_ERROR(SBE_FUNC "p9_sbe_sequence_drtm LQA SBE System Quiesce failed,"
+            "Either System Quiesce Achieved not true or procedure "
+            "failed RC=[0x%08X]",l_rc);
+        // check stop the system
+        // TODO RTC 164425 this needs to be replicated on any MPIPL Hwp failure
+        Target<TARGET_TYPE_PROC_CHIP > l_proc = plat_getChipTarget();
+        l_rc = putscom_abs_wrap(&l_proc, PERV_N3_LOCAL_FIR_OR,
+                                    N3_FIR_SYSTEM_CHECKSTOP_BIT);
+        if(l_rc != FAPI2_RC_SUCCESS)
+        {
+            // Scom failed
+            SBE_ERROR(SBE_FUNC "PutScom failed for REG PERV_N3_LOCAL_FIR");
+            // TODO - Store the response in Async Response
+            // RTC:149074
+        }
+    }
+fapi_try_exit:
+    if(fapi2::current_err)
+    {
+        l_rc = fapi2::current_err;
+    }
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepNoOpStartMpipl( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepNoOpStartMpipl"
+    SBE_ENTER(SBE_FUNC);
+    (void)SbeRegAccess::theSbeRegAccess().stateTransition(
+                                          SBE_ENTER_MPIPL_EVENT);
+    SBE_EXIT(SBE_FUNC);
+    return FAPI2_RC_SUCCESS;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepMpiplQuadPoweroff( sbeIstepHwp_t i_hwp)
+{
+    #define SBE_FUNC "istepMpiplQuadPoweroff"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+    if(g_sbeRole == SBE_ROLE_MASTER)
+    {
+        Target<TARGET_TYPE_PROC_CHIP > l_proc = plat_getChipTarget();
+        // Fetch the MASTER_CORE attribute
+        uint8_t l_coreId = 0;
+        FAPI_ATTR_GET(fapi2::ATTR_MASTER_CORE, l_proc, l_coreId);
+        // Construct the Master Core Target
+        fapi2::Target<fapi2::TARGET_TYPE_CORE > l_core(
+            plat_getTargetHandleByChipletNumber<fapi2::TARGET_TYPE_CORE>(
+            l_coreId + CORE_CHIPLET_OFFSET));
+        fapi2::Target<fapi2::TARGET_TYPE_EQ> l_quad =
+                                l_core.getParent<fapi2::TARGET_TYPE_EQ>();
+        SBE_EXEC_HWP(l_rc, i_hwp.eqHwp, l_quad)
+    }
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
+//----------------------------------------------------------------------------
+ReturnCode istepWithProcQuiesceLQASet( sbeIstepHwp_t i_hwp )
+{
+    #define SBE_FUNC "istepWithProcQuiesceLQASet"
+    SBE_ENTER(SBE_FUNC);
+    ReturnCode l_rc = FAPI2_RC_SUCCESS;
+    do
+    {
+        l_rc = istepWithProc(i_hwp);
+        if(l_rc == FAPI2_RC_SUCCESS)
+        {
+            //set the LQA Bit
+            // TODO RTC 164425 - Create another istep for Setting LQA bit after
+            // L2/L3 flush istep
+            Target<TARGET_TYPE_PROC_CHIP > l_proc = plat_getChipTarget();
+            l_rc = putscom_abs_wrap(&l_proc, PU_SECURITY_SWITCH_REGISTER_SCOM,
+                            PU_SECURITY_SWITCH_REGISTER_LOCAL_QUIESCE_ACHIEVED);
+            if(l_rc != FAPI2_RC_SUCCESS)
+            {
+                SBE_ERROR(SBE_FUNC "PutScom failed for PU_SECURITY_SWITCH_REGISTER_SCOM");
+                break;
+            }
+        }
+    }while(0);
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
 
diff --git a/src/sbefw/sbecmdiplcontrol.H b/src/sbefw/sbecmdiplcontrol.H
index 0aaa0684..c5ccf71e 100644
--- a/src/sbefw/sbecmdiplcontrol.H
+++ b/src/sbefw/sbecmdiplcontrol.H
@@ -39,6 +39,44 @@ namespace fapi2
     class ReturnCode;
 }
 
+// Major isteps which are supported
+typedef enum
+{
+    SBE_ISTEP2 = 2,
+    SBE_ISTEP_FIRST = SBE_ISTEP2,
+    SBE_ISTEP3 = 3,
+    SBE_ISTEP_LAST_SLAVE = SBE_ISTEP3,
+    SBE_ISTEP4 = 4,
+    SBE_ISTEP5 = 5,
+    SBE_ISTEP_LAST_MASTER = SBE_ISTEP5,
+}sbe_supported_steps_t;
+
+// Major MPIPL isteps which are supported
+static const uint32_t SBE_ISTEP_MPIPL_START         = 96;
+static const uint32_t SBE_ISTEP_MPIPL_CONTINUE      = 97;
+static const uint32_t MPIPL_START_MAX_SUBSTEPS      = 8;
+static const uint32_t MPIPL_CONTINUE_MAX_SUBSTEPS   = 3;
+
+// constants
+static const uint32_t ISTEP2_MAX_SUBSTEPS = 17;
+static const uint32_t ISTEP3_MAX_SUBSTEPS = 22;
+static const uint32_t ISTEP4_MAX_SUBSTEPS = 34;
+static const uint32_t ISTEP5_MAX_SUBSTEPS = 2;
+static const uint8_t ISTEP_MINOR_START = 1;
+static const uint8_t SLAVE_LAST_MINOR_ISTEP = 20;
+static const uint8_t ISTEP2_MINOR_START = 2;
+
+/**
+ * @brief Support function to execute specific istep
+ *
+ * @param[in] i_major Major Istep Number
+ * @param[in] i_minor Minor Istep Number
+ *
+ * @return FAPI2_RC_SUCCESS if success, else error code.
+ */
+fapi2::ReturnCode sbeExecuteIstep (uint8_t i_major, uint8_t i_minor);
+
+
 /**
   * @brief execute istep chipop (0xA101)
   *
diff --git a/src/sbefw/sbecmdmpipl.C b/src/sbefw/sbecmdmpipl.C
index 9e03f9b2..7db0f199 100644
--- a/src/sbefw/sbecmdmpipl.C
+++ b/src/sbefw/sbecmdmpipl.C
@@ -37,6 +37,7 @@
 #include "sbecmdmpipl.H"
 #include "sberegaccess.H"
 #include "sbefapiutil.H"
+#include "sbecmdiplcontrol.H"
 
 #include "p9_hcd_core_stopclocks.H"
 #include "p9_hcd_cache_stopclocks.H"
@@ -44,14 +45,12 @@
 
 using namespace fapi2;
 
-
 #ifdef SEEPROM_IMAGE
-// Using function pointer to force long call.
+    // Using function pointer to force long call.
 p9_hcd_cache_stopclocks_FP_t p9_hcd_cache_stopclocks_hwp = &p9_hcd_cache_stopclocks;
 p9_hcd_core_stopclocks_FP_t p9_hcd_core_stopclocks_hwp = &p9_hcd_core_stopclocks;
 #endif
 
-// TODO - RTC 133367
 ///////////////////////////////////////////////////////////////////////
 // @brief sbeEnterMpipl Sbe enter MPIPL function
 //
@@ -62,9 +61,11 @@ uint32_t sbeEnterMpipl(uint8_t *i_pArg)
     #define SBE_FUNC " sbeEnterMpipl "
     SBE_ENTER(SBE_FUNC);
     uint32_t l_rc = SBE_SEC_OPERATION_SUCCESSFUL;
+    uint32_t l_fapiRc = FAPI2_RC_SUCCESS;
     uint32_t len = 0;
     sbeRespGenHdr_t l_respHdr;
     l_respHdr.init();
+    sbeResponseFfdc_t l_ffdc;
 
     do
     {
@@ -72,15 +73,33 @@ uint32_t sbeEnterMpipl(uint8_t *i_pArg)
         l_rc = sbeUpFifoDeq_mult (len, NULL);
         CHECK_SBE_RC_AND_BREAK_IF_NOT_SUCCESS(l_rc);
 
-        // TODO via  RTC:123696  MPIPL Related procedure/steps
-        // Can send FFDC if MPIPL procedure fails
-        l_rc = sbeDsSendRespHdr( l_respHdr );
+        uint32_t l_minor = 1;
+        do
+        {
+            l_fapiRc = sbeExecuteIstep(SBE_ISTEP_MPIPL_START, l_minor);
+            if(l_fapiRc != FAPI2_RC_SUCCESS)
+            {
+                SBE_ERROR(SBE_FUNC "Failed in Mpipl Start in ChipOp Mode "
+                    "Minor: %d", l_minor);
+                l_respHdr.setStatus( SBE_PRI_GENERIC_EXECUTION_FAILURE,
+                                 SBE_SEC_GENERIC_FAILURE_IN_EXECUTION);
+                l_ffdc.setRc(l_fapiRc);
+                break;
+            }
+            ++l_minor;
+        }while(l_minor<=MPIPL_START_MAX_SUBSTEPS);
 
-        // set state to MPIPL Wait
-        (void)SbeRegAccess::theSbeRegAccess().
-               stateTransition(SBE_ENTER_MPIPL_EVENT);
+    }while(0);
 
+    // Create the Response to caller
+    do
+    {
+        // If there was a FIFO error, will skip sending the response,
+        // instead give the control back to the command processor thread
+        CHECK_SBE_RC_AND_BREAK_IF_NOT_SUCCESS(l_rc);
+        l_rc = sbeDsSendRespHdr( l_respHdr, &l_ffdc);
     }while(0);
+
     SBE_EXIT(SBE_FUNC);
     return l_rc;
     #undef SBE_FUNC
diff --git a/src/sbefw/sbecmdparser.C b/src/sbefw/sbecmdparser.C
index a7be57a6..e5a3bd7f 100644
--- a/src/sbefw/sbecmdparser.C
+++ b/src/sbefw/sbecmdparser.C
@@ -90,8 +90,7 @@ static sbeCmdStruct_t g_sbeIplControlCmdArray [] =
 {
     {sbeHandleIstep,
      SBE_CMD_EXECUTE_ISTEP,
-     PUT_HARDWARE_FENCED_STATE|SBE_FENCE_AT_RUNTIME|
-     SBE_FENCE_AT_DUMPING,
+     HARDWARE_FENCED_STATE|SBE_FENCE_AT_DUMPING,
      // TODO - Issue 157287 - Allow MPIIPL in Isteps state
     },
 };
diff --git a/src/sbefw/sberegaccess.C b/src/sbefw/sberegaccess.C
index 71b2b3bf..925c52ff 100644
--- a/src/sbefw/sberegaccess.C
+++ b/src/sbefw/sberegaccess.C
@@ -84,7 +84,7 @@ static const stateTransitionStr_t stateTransMap[SBE_MAX_TRANSITIONS] = {
     {SBE_STATE_ISTEP, SBE_RUNTIME_EVENT, SBE_STATE_RUNTIME},
     {SBE_STATE_ISTEP, SBE_FAILURE_EVENT, SBE_STATE_FAILURE},
     {SBE_STATE_ISTEP, SBE_QUIESCE_EVENT, SBE_STATE_QUIESCE},
-    {SBE_STATE_MPIPL, SBE_CONTINUE_MPIPL_EVENT, SBE_STATE_RUNTIME},
+    {SBE_STATE_MPIPL, SBE_RUNTIME_EVENT, SBE_STATE_RUNTIME},
     {SBE_STATE_MPIPL, SBE_DUMP_FAILURE_EVENT, SBE_STATE_DUMP},
     {SBE_STATE_MPIPL, SBE_QUIESCE_EVENT, SBE_STATE_QUIESCE},
     {SBE_STATE_RUNTIME, SBE_DUMP_FAILURE_EVENT, SBE_STATE_DUMP},