Support for Control Instruction

Change-Id: Ib6f1e15a199f08aeb48e2486f0be901832ff4127
RTC:128325
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/21787
Tested-by: Jenkins Server
Reviewed-by: Sachin Gupta <sgupta2m@in.ibm.com>
Reviewed-by: Santosh S. Puranik <santosh.puranik@in.ibm.com>
Reviewed-by: AMIT J. TENDOLKAR <amit.tendolkar@in.ibm.com>

12 files changed, 826 insertions, 5 deletions

diff --git a/sbe/sbefw/sbeSpMsg.H b/sbe/sbefw/sbeSpMsg.H
index ff49e2f2..7d8e3fb1 100644
--- a/sbe/sbefw/sbeSpMsg.H
+++ b/sbe/sbefw/sbeSpMsg.H
@@ -408,4 +408,72 @@ typedef struct
 
 extern sbeCmdRespHdr_t g_sbeCmdRespHdr;
 
+/**
+  * @brief structure for Control Instruction Chipop (0xA701) contents.
+  *
+  */
+typedef struct
+{
+    uint32_t           reserved:12;
+    sbeErrorMode       mode:4;
+    sbeCoreChipletId   coreChipletId:8;
+    sbeThreadNum       threadNum:4;
+    sbeThreadOps       threadOps:4;
+
+    /**
+      * @brief Validate input arguments
+      *
+      * @return bool, true if the validation is success, else false for
+      * validation failure
+      */
+    bool validateInputArgs()
+    {
+        bool l_validatePassFlag = true;
+        // Validate Thread Command / Thread Num / Error Mode
+        if((threadOps > THREAD_SRESET_INS) ||
+           (mode > IGNORE_HW_ERRORS) ||
+           !((threadNum <= SMT4_THREAD3) || (threadNum == SMT4_THREAD_ALL)))
+        {
+            SBE_ERROR(SBE_FUNC "Invalid Parameter by User, ThreadOps[%d] "
+                "mode[%d] ThreadNum[%d]", threadOps, mode, threadNum);
+            l_validatePassFlag = false;
+        }
+        return l_validatePassFlag; 
+    }
+
+    /**
+      * @brief Process the input to find out core/thread ids to iterate
+      *        over HWP for the internal business logic
+      *
+      * @param[out] o_core, Core Id to start with in the iteration
+      * @param[out] o_coreCntMax, Core Max count to iterate, start from o_core
+      * @param[out] o_threadCnt, Thread Num to start with in the iteration
+      * @param[out] o_threadCntMax, Thread Max Num to iterate, start from
+      *             o_threadCnt
+      *
+      * @return void
+      */
+    void processInputDataToIterate(uint8_t & o_core, uint8_t & o_coreCntMax,
+                               uint8_t & o_threadCnt, uint8_t & o_threadCntMax)
+    {
+        //Default Init
+        o_threadCnt = SMT4_THREAD0;
+        o_threadCntMax = SMT4_THREAD_MAX;
+
+        o_core = SMT4_CORE0_ID;
+        o_coreCntMax = SMT4_CORE_ID_MAX;
+
+        if( SMT4_ALL_CORES != coreChipletId )
+        {
+            o_core = coreChipletId;
+            o_coreCntMax = coreChipletId;
+        }
+        if( SMT4_THREAD_ALL != threadNum )
+        {
+            o_threadCnt = threadNum;
+            o_threadCntMax = threadNum;
+        }
+    }
+}sbeCntlInstRegMsgHdr_t;
+
 #endif // __SBEFW_SBESP_MSG_H
diff --git a/sbe/sbefw/sbe_sp_intf.H b/sbe/sbefw/sbe_sp_intf.H
index c2f5b546..7f5fac19 100644
--- a/sbe/sbefw/sbe_sp_intf.H
+++ b/sbe/sbefw/sbe_sp_intf.H
@@ -287,6 +287,48 @@ enum sbeSramAccessMode
     CIRCULAR_MODE  = 0x3,
 };
 
+/**
+ * @brief Error Mode enum
+ */
+enum sbeErrorMode
+{
+    EXIT_ON_FIRST_ERROR = 0x0,   // Bail out on first error
+    IGNORE_HW_ERRORS    = 0x01,  // Attempt best case
+};
+
+/**
+ * @brief Core Chiplet Id Enum
+ */
+enum sbeCoreChipletId
+{
+    SMT4_CORE0_ID       = 0x20,
+    SMT4_CORE_ID_MAX    = 0x38,
+    SMT4_ALL_CORES      = 0xFF,
+};
+
+/**
+ * @brief Thread Num Enum
+ */
+enum sbeThreadNum
+{
+    SMT4_THREAD0     = 0x0,
+    SMT4_THREAD1     = 0x1,
+    SMT4_THREAD2     = 0x2,
+    SMT4_THREAD3     = 0x3,
+    SMT4_THREAD_MAX  = 0x4,
+    SMT4_THREAD_ALL  = 0xF,
+};
+
+/**
+ * @brief Thread Operation Enum
+ */
+enum sbeThreadOps
+{
+    THREAD_START_INS       = 0x0,
+    THREAD_STOP_INS        = 0x1,
+    THREAD_STEP_INS        = 0x2,
+    THREAD_SRESET_INS      = 0x3,
+};
 
 #ifdef __cplusplus
 }
diff --git a/sbe/sbefw/sbecmdcntlinst.C b/sbe/sbefw/sbecmdcntlinst.C
new file mode 100644
index 00000000..09eb8232
--- /dev/null
+++ b/sbe/sbefw/sbecmdcntlinst.C
@@ -0,0 +1,200 @@
+/*
+ * @file: ppe/sbe/sbefw/sbecmdcntlinst.C
+ *
+ * @brief This file contains the SBE Control Instruction chipOps
+ *
+ */
+
+#include "sbecmdcntlinst.H"
+#include "sbefifo.H"
+#include "sbe_sp_intf.H"
+#include "sbetrace.H"
+#include "sbeFifoMsgUtils.H"
+
+#include "fapi2.H"
+#include "p9_thread_control.H"
+
+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;
+
+/* @brief Map User Thread Command to Hwp ThreadCommands Enum */
+ThreadCommands getThreadCommand(const sbeCntlInstRegMsgHdr_t & i_req)
+{
+    ThreadCommands l_cmd = PTC_CMD_START;
+    switch(i_req.threadOps)
+    {
+        case THREAD_START_INS:  l_cmd = PTC_CMD_START;  break;
+        case THREAD_STOP_INS:   l_cmd = PTC_CMD_STOP;   break;
+        case THREAD_STEP_INS:   l_cmd = PTC_CMD_STEP;   break;
+        case THREAD_SRESET_INS: l_cmd = PTC_CMD_SRESET; break;
+    }
+    return l_cmd;
+}
+
+/* @brief Map User Mode Command to Hwp Warn Check flag */
+inline bool getWarnCheckFlag(const sbeCntlInstRegMsgHdr_t & i_req)
+{
+    bool l_warnCheck = false;
+    if( EXIT_ON_FIRST_ERROR != i_req.mode )
+    {
+        l_warnCheck = true;
+    }
+    return l_warnCheck;
+}
+
+///////////////////////////////////////////////////////////////////////
+// @brief sbeCntlInst Sbe control instructions function
+//
+// @return  RC from the underlying FIFO utility
+///////////////////////////////////////////////////////////////////////
+uint32_t sbeCntlInst(uint8_t *i_pArg)
+{
+    #define SBE_FUNC " sbeCntlInst "
+    SBE_ENTER(SBE_FUNC);
+
+    uint32_t l_rc = SBE_SEC_OPERATION_SUCCESSFUL;
+    ReturnCode l_fapiRc = FAPI2_RC_SUCCESS;
+    sbeRespGenHdr_t l_respHdr;
+    l_respHdr.init();
+    sbeResponseFfdc_t l_ffdc;
+
+    // Create the req struct for Control Instructions Chip-op
+    sbeCntlInstRegMsgHdr_t l_req = {0};
+
+    do
+    {
+        // Get the Req Struct Data sbeCntlInstRegMsgHdr_t from upstream Fifo
+        uint32_t l_len2dequeue  = sizeof(l_req) / sizeof(uint32_t);
+        l_rc = sbeUpFifoDeq_mult (l_len2dequeue, (uint32_t *)&l_req, true);
+
+        // If FIFO failure
+        if ( SBE_SEC_OPERATION_SUCCESSFUL != l_rc )
+        {
+            // Let command processor routine to handle the RC.
+            break;
+        }
+
+        SBE_DEBUG("mode[0x%04X] coreChipletId[0x%08X] threadNum[0x%04X] "
+            "threadOps[0x%04X] ", l_req.mode, l_req.coreChipletId,
+            l_req.threadNum, l_req.threadOps);
+
+        // Validate Input Args
+        if( false == l_req.validateInputArgs())
+        {
+            SBE_ERROR(SBE_FUNC "ValidateAndMapInputArgs failed");
+            l_respHdr.setStatus( SBE_PRI_INVALID_DATA,
+                                 SBE_SEC_GENERIC_FAILURE_IN_EXECUTION );
+            break;
+        }
+
+        // Fetch HWP mapped values
+        bool l_warnCheck = getWarnCheckFlag(l_req);
+        ThreadCommands l_cmd = getThreadCommand(l_req);
+
+        // Default assignment not required since it is assigned below
+        uint8_t l_core, l_coreCntMax;
+        uint8_t l_threadCnt, l_threadCntMax;
+
+        l_req.processInputDataToIterate(l_core, l_coreCntMax,
+                                        l_threadCnt, l_threadCntMax);
+        fapi2::buffer<uint64_t> l_data64;
+        uint64_t l_state;
+        do
+        {
+            fapi2::Target<fapi2::TARGET_TYPE_CORE> l_coreTgt(
+                        plat_getTargetHandleByChipletNumber(l_core));
+
+            uint8_t l_thread = l_threadCnt;
+            do
+            {
+                // Call the Procedure
+                l_fapiRc = p9_thread_control(
+                                    l_coreTgt,
+                                    (SINGLE_THREAD_BIT_MASK >> l_thread),
+                                    l_cmd, l_warnCheck,
+                                    l_data64, l_state);
+
+                if(l_fapiRc != FAPI2_RC_SUCCESS)
+                {
+                    SBE_ERROR(SBE_FUNC "Failed for Core[%d] Thread [%d] "
+                        "Cmd[%d] Mode[%d]", l_core, l_thread, l_req.threadOps,
+                        l_req.mode);
+                    if(IGNORE_HW_ERRORS == l_req.mode)
+                    {
+                        // No need to delete the l_fapiRc handle,it will get
+                        // over-written
+                        SBE_DEBUG(SBE_FUNC "Continuing in case of HW Errors"
+                            " As user has passed to ignore errors.");
+                        continue;
+                    }
+                    else
+                    {
+                        SBE_ERROR(SBE_FUNC "Breaking out, since User has "
+                            "Selected the mode to exit on first error.");
+                        l_respHdr.setStatus(SBE_PRI_GENERIC_EXECUTION_FAILURE,
+                                         SBE_SEC_GENERIC_FAILURE_IN_EXECUTION);
+                        l_ffdc.setRc(l_fapiRc);
+                        break;
+                    }
+                }
+            }while(++l_thread < l_threadCntMax);
+
+            // If FapiRc from the inner loop (thread loop), just break here
+            if ( l_fapiRc )
+            {
+                break; // From core while loop
+            }
+        }while(++l_core < l_coreCntMax);
+
+    }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
+        if ( SBE_SEC_OPERATION_SUCCESSFUL != l_rc)
+        {
+            break;
+        }
+
+        uint32_t l_dist2Hdr = 1;
+        // Now enqueue the minimum response header
+        uint32_t l_len = sizeof(l_respHdr) / sizeof(uint32_t);
+        l_rc = sbeDownFifoEnq_mult(l_len, (uint32_t *)(&l_respHdr) );
+        if ( SBE_SEC_OPERATION_SUCCESSFUL != l_rc)
+        {
+            break;
+        }
+
+        l_dist2Hdr += l_len;
+
+        // Enqueue FFDC data if there is one
+        if( l_ffdc.getRc() )
+        {
+            l_len = sizeof(l_ffdc) / sizeof(uint32_t);
+            l_rc = sbeDownFifoEnq_mult ( l_len, (uint32_t *)(&l_ffdc) );
+            if ( SBE_SEC_OPERATION_SUCCESSFUL != l_rc)
+            {
+                break;
+            }
+
+            l_dist2Hdr += l_len;
+        }
+
+        l_len = sizeof(l_dist2Hdr) / sizeof(uint32_t);
+        l_rc = sbeDownFifoEnq_mult ( l_len, &l_dist2Hdr);
+        if ( l_rc )
+        {
+            break;
+        }
+    }while(0);
+
+    SBE_EXIT(SBE_FUNC);
+    return l_rc;
+    #undef SBE_FUNC
+}
+
diff --git a/sbe/sbefw/sbecmdcntlinst.H b/sbe/sbefw/sbecmdcntlinst.H
new file mode 100644
index 00000000..61323703
--- /dev/null
+++ b/sbe/sbefw/sbecmdcntlinst.H
@@ -0,0 +1,22 @@
+/*
+ * @file: ppe/sbe/sbefw/sbecmdcntlinst.H
+ *
+ * @brief This file contains the Interfaces for Control Instructions chip-ops
+ *
+ */
+
+#ifndef __SBEFW_SBECMDCNTLINST_H
+#define __SBEFW_SBECMDCNTLINST_H
+
+#include <stdint.h>
+
+/**
+  * @brief sbeCntlInst : Implements SBE Control instructions ChipOp
+  *
+  * @param[in] i_pArg Buffer to be passed to the function (not used as of now)
+  *
+  * @return    Rc from the FIFO access utility
+  */
+uint32_t sbeCntlInst (uint8_t *i_pArg);
+
+#endif /* __SBEFW_SBECMDCNTLINST_H */
diff --git a/sbe/sbefw/sbecmdgeneric.C b/sbe/sbefw/sbecmdgeneric.C
index eda09d88..8fe7b6a2 100644
--- a/sbe/sbefw/sbecmdgeneric.C
+++ b/sbe/sbefw/sbecmdgeneric.C
@@ -41,6 +41,9 @@ sbeCapabilityRespMsg::sbeCapabilityRespMsg()
                                 PUT_MEMORY_SUPPPORTED |
                                 GET_SRAM_OCC_SUPPPORTED |
                                 PUT_SRAM_OCC_SUPPPORTED;
+
+    capability[INSTRUCTION_CTRL_CAPABILITY_START_IDX] =
+                                CONTROL_INSTRUCTIONS_SUPPPORTED;
 }
 // Functions
 //----------------------------------------------------------------------------
diff --git a/sbe/sbefw/sbecmdparser.C b/sbe/sbefw/sbecmdparser.C
index f71e5a96..6769be77 100644
--- a/sbe/sbefw/sbecmdparser.C
+++ b/sbe/sbefw/sbecmdparser.C
@@ -12,6 +12,7 @@
 #include "sbecmdmemaccess.H"
 #include "sbecmdcntrldmt.H"
 #include "sbecmdsram.H"
+#include "sbecmdcntlinst.H"
 #include "sbetrace.H"
 #include "sbe_sp_intf.H"
 #include "sbeHostMsg.H"
@@ -103,6 +104,18 @@ static sbeCmdStruct_t g_sbeMemoryAccessCmdArray [] =
 };
 
 //////////////////////////////////////////////////////////////
+// @brief g_sbeInstructionCntlCmdArray
+//
+//////////////////////////////////////////////////////////////
+static sbeCmdStruct_t g_sbeInstructionCntlCmdArray[] =
+{
+    {sbeCntlInst,
+     SBE_CMD_CONTROL_INSTRUCTIONS,
+     SBE_FENCE_AT_CONTINUOUS_IPL,
+    },
+};
+
+//////////////////////////////////////////////////////////////
 // @brief g_sbeCoreStateControlCmdArray
 //
 //////////////////////////////////////////////////////////////
@@ -120,7 +133,7 @@ uint8_t sbeGetCmdStructAttr (const uint8_t  i_cmdClass,
                                    sbeCmdStruct_t **o_ppCmd)
 {
     #define SBE_FUNC " sbeGetCmdStructAttr "
-    SBE_DEBUG(SBE_FUNC);
+    SBE_DEBUG(SBE_FUNC "i_cmdClass [0x%08X]", i_cmdClass);
     uint8_t l_numCmds  = 0;
     *o_ppCmd = NULL;
 
@@ -151,6 +164,12 @@ uint8_t sbeGetCmdStructAttr (const uint8_t  i_cmdClass,
             *o_ppCmd  = (sbeCmdStruct_t*)g_sbeMemoryAccessCmdArray;
             break;
 
+        case SBE_CMD_CLASS_INSTRUCTION_CONTROL:
+            l_numCmds = sizeof(g_sbeInstructionCntlCmdArray) /
+                        sizeof(sbeCmdStruct_t);
+            *o_ppCmd  = (sbeCmdStruct_t*)g_sbeInstructionCntlCmdArray;
+            break;
+
         // PSU Commands
         case SBE_PSU_CMD_CLASS_CORE_STATE:
             l_numCmds = sizeof(g_sbeCoreStateControlCmdArray) /
diff --git a/sbe/sbefw/sbefwfiles.mk b/sbe/sbefw/sbefwfiles.mk
index 95d4bdf9..32826a7d 100644
--- a/sbe/sbefw/sbefwfiles.mk
+++ b/sbe/sbefw/sbefwfiles.mk
@@ -14,7 +14,7 @@ SBEFW-CPP-SOURCES += sbeHostUtils.C
 SBEFW-CPP-SOURCES += sbecmdcntrldmt.C
 SBEFW-CPP-SOURCES += sbecmdsram.C
 SBEFW-CPP-SOURCES += sberegaccess.C
-
+SBEFW-CPP-SOURCES += sbecmdcntlinst.C
 SBEFW-C-SOURCES =
 SBEFW-S-SOURCES =
 
diff --git a/sbe/test/test.xml b/sbe/test/test.xml
index f5fdb966..6fb16651 100755
--- a/sbe/test/test.xml
+++ b/sbe/test/test.xml
@@ -3,14 +3,13 @@
 <integrationtest>
 	<platform startsimargs="--notar --norun --sim_parms -nre">
 		<machine>%%machine%%</machine>
-
         <test>
-           <!-- Set IVPR to point to sbe start address -->
           <include>../simics/targets/p9_nimbus/sbeTest/testIstep.xml</include>
           <include>../simics/targets/p9_nimbus/sbeTest/testScom.xml</include>
           <include>../simics/targets/p9_nimbus/sbeTest/testGeneric.xml</include>
           <include>../simics/targets/p9_nimbus/sbeTest/testPutGetMem.xml</include>
           <include>../simics/targets/p9_nimbus/sbeTest/testSram.xml</include>
+          <include>../simics/targets/p9_nimbus/sbeTest/testCntlInstruction.xml</include>
           <testcase>
               <simcmd>sbe-trace 0</simcmd>
           </testcase>
diff --git a/sbe/test/testCntlInstruction.py b/sbe/test/testCntlInstruction.py
new file mode 100644
index 00000000..df0a9d46
--- /dev/null
+++ b/sbe/test/testCntlInstruction.py
@@ -0,0 +1,453 @@
+import sys
+sys.path.append("targets/p9_nimbus/sbeTest" )
+import testUtil
+err = False
+#from testWrite import *
+
+LOOP_COUNT = 1
+
+#Invalid Input
+INST_INVALID_TESTDATA = [0,0,0,3,
+                         0,0,0xa7,0x01,
+                         0,1,0x20,0xee]
+
+
+INST_INVALID_EXPDATA_ERR = [0xc0,0xde,0xa7,0x01,
+                            0x00,0x02,0x00,0x0A,
+                            0x00,0x00,0x00,0x03]
+
+# STOP Ins
+# core 0 thread 0 STOP WARN FLAG as true
+INST_STOP_0_0_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x01]
+
+# core 0 thread 1 STOP WARN FLAG as true
+INST_STOP_0_1_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x11]
+
+# core 0 thread 2 STOP WARN FLAG as true
+INST_STOP_0_2_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x21]
+
+# core 0 thread 3 STOP with WARN FLAG as true
+INST_STOP_0_3_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x31]
+
+# core 0 thread 0 STOP WARN FLAG as false
+INST_STOP_0_0_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x01]
+
+# core 0 thread 1 STOP WARN FLAG as false
+INST_STOP_0_1_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x11]
+
+# core 0 thread 2 STOP WARN FLAG as false
+INST_STOP_0_2_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x21]
+
+# core 0 thread 3 STOP WARN FLAG as false
+INST_STOP_0_3_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x31]
+
+# Stop All thread in Core0 with warn flag true
+INST_STOP0_ALL_TESTDATA_WITH_WARN_FLG =      [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,1,0x20,0xf1]
+
+# Stop All thread in Core0 with warn flag false
+INST_STOP0_ALL_TESTDATA_WITHOUT_WARN_FLG =      [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,0,0x20,0xf1]
+
+
+# START Ins
+# core 0 thread 0 START WARN FLAG as true
+INST_START_0_0_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x00]
+
+# core 0 thread 1 START WARN FLAG as true
+INST_START_0_1_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x10]
+
+# core 0 thread 2 START WARN FLAG as true
+INST_START_0_2_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x20]
+
+# core 0 thread 3 START with WARN FLAG as true
+INST_START_0_3_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,1,0x20,0x30]
+
+# core 0 thread 0 START WARN FLAG as false
+INST_START_0_0_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x00]
+
+# core 0 thread 1 START WARN FLAG as false
+INST_START_0_1_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x10]
+
+# core 0 thread 2 START WARN FLAG as false
+INST_START_0_2_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x20]
+
+# core 0 thread 3 START WARN FLAG as false
+INST_START_0_3_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                               0,0,0xa7,0x01,
+                                               0,0,0x20,0x30]
+
+# Start All thread in Core0 with warn flag true
+INST_START0_ALL_TESTDATA_WITH_WARN_FLG =      [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,1,0x20,0xf0]
+
+# Start All thread in Core0 with warn flag false
+INST_START0_ALL_TESTDATA_WITHOUT_WARN_FLG =      [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,0,0x20,0xf0]
+
+# STEP Ins
+# core 0 thread 0 STEP WARN FLAG as true
+INST_STEP_0_0_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x02]
+
+# core 0 thread 1 STEP WARN FLAG as true
+INST_STEP_0_1_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x12]
+
+# core 0 thread 2 STEP WARN FLAG as true
+INST_STEP_0_2_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x22]
+
+# core 0 thread 3 STEP with WARN FLAG as true
+INST_STEP_0_3_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x32]
+
+# core 0 thread 0 STEP WARN FLAG as false
+INST_STEP_0_0_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x02]
+
+# core 0 thread 1 STEP WARN FLAG as false
+INST_STEP_0_1_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x12]
+
+# core 0 thread 2 STEP WARN FLAG as false
+INST_STEP_0_2_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x22]
+
+# core 0 thread 3 STEP WARN FLAG as false
+INST_STEP_0_3_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x32]
+
+# Step All thread in Core0 with warn flag true
+INST_STEP0_ALL_TESTDATA_WITH_WARN_FLG =      [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,1,0x20,0xf2]
+
+# Step All thread in Core0 with warn flag false
+INST_STEP0_ALL_TESTDATA_WITHOUT_WARN_FLG =   [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,0,0x20,0xf2]
+
+# SRESET Ins
+# core 0 thread 0 SRESET WARN FLAG as true
+INST_SRESET_0_0_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x03]
+
+# core 0 thread 1 SRESET WARN FLAG as true
+INST_SRESET_0_1_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x13]
+
+# core 0 thread 2 SRESET WARN FLAG as true
+INST_SRESET_0_2_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x23]
+
+# core 0 thread 3 SRESET with WARN FLAG as true
+INST_SRESET_0_3_TESTDATA_WITH_WARN_FLG  =  [0,0,0,0x03,
+                                          0,0,0xa7,0x01,
+                                          0,1,0x20,0x33]
+
+# core 0 thread 0 SRESET WARN FLAG as false
+INST_SRESET_0_0_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x03]
+
+# core 0 thread 1 SRESET WARN FLAG as false
+INST_SRESET_0_1_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x13]
+
+# core 0 thread 2 SRESET WARN FLAG as false
+INST_SRESET_0_2_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x23]
+
+# core 0 thread 3 SRESET WARN FLAG as false
+INST_SRESET_0_3_TESTDATA_WITHOUT_WARN_FLG  =  [0,0,0,0x03,
+                                             0,0,0xa7,0x01,
+                                             0,0,0x20,0x33]
+
+# Sreset All thread in Core0 with warn flag true
+INST_SRESET0_ALL_TESTDATA_WITH_WARN_FLG =      [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,1,0x20,0xf3]
+
+# Sreset All thread in Core0 with warn flag false
+INST_SRESET0_ALL_TESTDATA_WITHOUT_WARN_FLG =   [0,0,0,0x03,
+                                              0,0,0xa7,0x01,
+                                              0,0,0x20,0xf3]
+
+
+INST_EXPDATA  =     [0xc0,0xde,0xa7,0x01,
+                     0x0,0x0,0x0,0x0,
+                     0x00,0x0,0x0,0x03]
+
+INST_EXPDATA_ERR = [0xc0,0xde,0xa7,0x01,
+                    0x00,0xFE,0x00,0x0A,
+                    0x00,0x00,0x00,0x03]
+
+STOP_INST_EXPDATA_ERR_WTH_FFDC = [0xc0,0xde,0xa7,0x01,
+                                  0x00,0xFE,0x00,0x0A,
+                                  0xFF,0xDC,0x00,0x03,
+                                  0x00,0x00,0x00,0x00,
+                                  0x00,0xCE,0xBC,0xB2,
+                                  0x00,0x00,0x00,0x06]
+
+START_INST_EXPDATA_ERR_WTH_FFDC = [0xc0,0xde,0xa7,0x01,
+                                  0x00,0xFE,0x00,0x0A,
+                                  0xFF,0xDC,0x00,0x03,
+                                  0x00,0x00,0x00,0x00,
+                                  0x00,0x25,0x64,0xDB,
+                                  0x00,0x00,0x00,0x06]
+
+STEP_INST_EXPDATA_ERR_WTH_FFDC = [0xc0,0xde,0xa7,0x01,
+                                  0x00,0xFE,0x00,0x0A,
+                                  0xFF,0xDC,0x00,0x03,
+                                  0x00,0x00,0x00,0x00,
+                                  0x00,0x0D,0x06,0x8E,
+                                  0x00,0x00,0x00,0x06]
+
+# MAIN Test Run Starts Here...
+#-------------------------------------------------
+def main( ):
+    testUtil.runCycles( 10000000 )
+    #Try an invalid data case
+    testUtil.writeUsFifo( INST_INVALID_TESTDATA )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_INVALID_EXPDATA_ERR )
+    testUtil.readEot( )
+
+    # Control Instruction Message - Stop
+    testUtil.writeUsFifo( INST_STOP_0_0_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_1_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_2_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_3_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_0_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STOP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_1_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STOP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_2_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STOP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP_0_3_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STOP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+
+    #stop all thread in core0
+    testUtil.writeUsFifo( INST_STOP0_ALL_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STOP0_ALL_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STOP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+
+    # Control Instruction Message - Start
+    testUtil.writeUsFifo( INST_START_0_0_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START_0_1_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START_0_2_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START_0_3_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+
+    testUtil.writeUsFifo( INST_START_0_0_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( START_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START_0_1_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( START_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START_0_2_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( START_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START_0_3_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( START_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+
+    #start all thread in core0
+    testUtil.writeUsFifo( INST_START0_ALL_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_START0_ALL_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( START_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+
+    # Control Instruction Message - Step
+    testUtil.writeUsFifo( INST_STEP_0_0_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_1_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_2_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_3_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_0_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STEP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_1_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STEP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_2_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STEP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP_0_3_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STEP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+
+    #step all thread in core0
+    testUtil.writeUsFifo( INST_STEP0_ALL_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_STEP0_ALL_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( STEP_INST_EXPDATA_ERR_WTH_FFDC )
+    testUtil.readEot( )
+
+    # Control Instruction Message - Sreset
+    testUtil.writeUsFifo( INST_SRESET_0_0_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_1_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_2_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_3_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_0_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_1_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_2_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET_0_3_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+
+    #step all thread in core0
+    testUtil.writeUsFifo( INST_SRESET0_ALL_TESTDATA_WITH_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+    testUtil.writeUsFifo( INST_SRESET0_ALL_TESTDATA_WITHOUT_WARN_FLG )
+    testUtil.writeEot( )
+    testUtil.readDsFifo( INST_EXPDATA )
+    testUtil.readEot( )
+
+
+#-------------------------------------------------
+# Calling all test code
+#-------------------------------------------------
+main()
+
+if err:
+    print ("\nTest Suite completed with error(s)")
+    #sys.exit(1)
+else:
+    print ("\nTest Suite completed with no errors")
+    #sys.exit(0);
+
diff --git a/sbe/test/testCntlInstruction.xml b/sbe/test/testCntlInstruction.xml
new file mode 100755
index 00000000..1231ac86
--- /dev/null
+++ b/sbe/test/testCntlInstruction.xml
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+      <testcase>
+          <simcmd>run-python-file targets/p9_nimbus/sbeTest/testCntlInstruction.py</simcmd>
+          <exitonerror>yes</exitonerror>
+      </testcase>
+
diff --git a/sbe/test/testGetCapabilities.py b/sbe/test/testGetCapabilities.py
index d7616eb7..4fb13a7f 100755
--- a/sbe/test/testGetCapabilities.py
+++ b/sbe/test/testGetCapabilities.py
@@ -21,7 +21,7 @@ EXPDATA2 = [0xa4,0x0,0x0,0x0f, #GetMemPba/PutMemPba/GetSramOcc/PutSramOcc
            0x0,0x0,0x0,0x0,
            0x0,0x0,0x0,0x0,
            0x0,0x0,0x0,0x0,
-           0x0,0x0,0x0,0x0,
+           0xa7,0x0,0x0,0x1, #  control Instruction
            0x00,0x0,0x0,0x0];
 
 EXPDATA3 = [0xa8,0x0,0x0,0x02, #getcapability
diff --git a/sbe/test/testIstep.xml b/sbe/test/testIstep.xml
index 07d92cd2..44de859a 100644
--- a/sbe/test/testIstep.xml
+++ b/sbe/test/testIstep.xml
@@ -261,6 +261,14 @@
           <simcmd>sbe-istep 4 31</simcmd>
           <exitonerror>yes</exitonerror>
       </testcase>
+      <testcase>
+          <simcmd>sbe-istep 5 1</simcmd>
+          <exitonerror>yes</exitonerror>
+      </testcase>
+      <testcase>
+          <simcmd>sbe-istep 5 2</simcmd>
+          <exitonerror>yes</exitonerror>
+      </testcase>
       <!-- Invalid Istep Test case -->
       <testcase>
           <simcmd>run-python-file targets/p9_nimbus/sbeTest/testIstepInvalid.py</simcmd>