Tod init and tod setup L2 procedures

Change-Id: I29fc21a3ec2ba9630413760e6532bd8952a4b594
Original-Change-Id: Ia5497a819a11d500436d5bced1609881642288e3
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/21670
Tested-by: Hostboot CI
Tested-by: PPE CI
Tested-by: Jenkins Server
Reviewed-by: James N. Klazynski <jklazyns@us.ibm.com>
Reviewed-by: Thi N. Tran <thi@us.ibm.com>
Reviewed-by: Jennifer A. Stofer <stofer@us.ibm.com>
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/29256
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Tested-by: FSP CI Jenkins <fsp-CI-jenkins+hostboot@us.ibm.com>
Reviewed-by: Daniel M. Crowell <dcrowell@us.ibm.com>

4 files changed, 1376 insertions, 47 deletions

diff --git a/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.C b/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.C
index dd50c9324..8964743a4 100644
--- a/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.C
+++ b/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.C
@@ -31,35 +31,946 @@
 // *HWP HWP Owner Christina Graves clgraves@us.ibm.com
 // *HWP FW Owner: Thi Tran thi@us.ibm.com
 // *HWP Team: Nest
-// *HWP Level: 1
+// *HWP Level: 2
 // *HWP Consumed by: SBE
 //
 //--------------------------------------------------------------------------
 
-
 //--------------------------------------------------------------------------
 // Includes
 //--------------------------------------------------------------------------
 #include <p9_tod_setup.H>
 
-extern "C" {
+extern "C"
+{
 
-//--------------------------------------------------------------------------
-//  HWP entry point
-//--------------------------------------------------------------------------
+    //--------------------------------------------------------------------------
+    //  HWP entry point
+    //--------------------------------------------------------------------------
     fapi2::ReturnCode p9_tod_setup(
         tod_topology_node* i_tod_node,
         const p9_tod_setup_tod_sel i_tod_sel,
         const p9_tod_setup_osc_sel i_osc_sel)
     {
+        fapi2::ATTR_IS_MPIPL_Type is_mpipl = 0x00;
+        const fapi2::Target<fapi2::TARGET_TYPE_SYSTEM> FAPI_SYSTEM;
         // mark HWP entry
         FAPI_DBG("Entering ...\n");
 
+        FAPI_INF("Configuring %s topology (OSC0 is %s, OSC1 is %s)",
+                 (i_tod_sel == TOD_PRIMARY) ? "Primary" : "Secondary",
+                 (i_osc_sel == TOD_OSC_0             ||
+                  i_osc_sel == TOD_OSC_0_AND_1       ||
+                  i_osc_sel == TOD_OSC_0_AND_1_SEL_0 ||
+                  i_osc_sel == TOD_OSC_0_AND_1_SEL_1) ? "connected" : "not connected",
+                 (i_osc_sel == TOD_OSC_1             ||
+                  i_osc_sel == TOD_OSC_0_AND_1       ||
+                  i_osc_sel == TOD_OSC_0_AND_1_SEL_0 ||
+                  i_osc_sel == TOD_OSC_0_AND_1_SEL_1) ? "connected" : "not connected");
+
+        FAPI_TRY(calculate_node_delays(i_tod_node), "Failure calculating TOD delays!");
+
+        display_tod_nodes(i_tod_node, 0);
+
+        FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_IS_MPIPL, FAPI_SYSTEM, is_mpipl),
+                 "fapiGetAttribute of ATTR_IS_MPIPL failed!");
+
+        // If there is a previous topology, it needs to be cleared
+        FAPI_TRY(clear_tod_node(i_tod_node, i_tod_sel, is_mpipl),
+                 "Failure clearing previous TOD configuration!");
 
-        /*fapi_try_exit:*/
+        //Start configuring each node; (configure_tod_node will recurse on each child)
+        FAPI_TRY(configure_tod_node(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring TOD!");
+
+    fapi_try_exit:
         FAPI_DBG("Exiting...");
         return fapi2::current_err;
     }
 
-} // extern "C"
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode clear_tod_node(tod_topology_node*             i_tod_node,
+                                     const p9_tod_setup_tod_sel   i_tod_sel,
+                                     const fapi2::ATTR_IS_MPIPL_Type i_is_mpipl)
+    {
+        fapi2::buffer<uint64_t> data(0x0);
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+        uint32_t port_ctrl_reg = 0;
+        uint32_t port_ctrl_check_reg = 0;
+        char l_targetStr[fapi2::MAX_ECMD_STRING_LEN];
+
+        fapi2::toString(target, l_targetStr, fapi2::MAX_ECMD_STRING_LEN);
+        FAPI_INF("Clearing previous %s topology from %s",
+                 (i_tod_sel == TOD_PRIMARY) ? "Primary" : "Secondary", l_targetStr);
+
+        if (i_tod_sel == TOD_PRIMARY)
+        {
+            FAPI_DBG("PERV_TOD_PRI_PORT_0_CTRL_REG and PERV_TOD_SEC_PORT_0_CTRL_REG will be cleared.");
+            port_ctrl_reg = PERV_TOD_PRI_PORT_0_CTRL_REG;
+            port_ctrl_check_reg = PERV_TOD_SEC_PORT_0_CTRL_REG;
+        }
+        else // (i_tod_sel==TOD_SECONDARY)
+        {
+            FAPI_DBG("PERV_TOD_PRI_PORT_1_CTRL_REG and PERV_TOD_SEC_PORT_1_CTRL_REG will be cleared.");
+            port_ctrl_reg = PERV_TOD_SEC_PORT_1_CTRL_REG;
+            port_ctrl_check_reg = PERV_TOD_PRI_PORT_1_CTRL_REG;
+        }
+
+        FAPI_TRY(fapi2::putScom(*target, port_ctrl_reg, data), "fapiPutScom error for port_ctrl_reg SCOM.");
+        FAPI_TRY(fapi2::putScom(*target, port_ctrl_check_reg, data),
+                 "fapiPutScom error for port_ctrl_check_reg SCOM.");
+
+        if (i_is_mpipl)
+        {
+            FAPI_INF("MPIPL: switch TOD to 'Not Set' state");
+            data.flush<0>();
+
+            // Generate TType#5 (formats defined in section "TType Fabric Interface" in the TOD workbook)
+            data.setBit<5>().setBit<56>();
+            FAPI_TRY(fapi2::putScom(*target, PERV_TOD_RX_TTYPE_CTRL_REG, data),
+                     "Could not write PERV_TOD_RX_TTYPE_CTRL_REG");
+
+            FAPI_INF("MPIPL: switch all other TODs to 'Not Set' state");
+            data.flush<0>().setBit<PERV_TOD_TX_TTYPE_5_REG_TRIGGER>();
+            FAPI_TRY(fapi2::putScom(*target, PERV_TOD_TX_TTYPE_5_REG, data),
+                     "Could not write PERV_TOD_TX_TTYPE_5_REG");
+        }
+        else
+        {
+            FAPI_INF("Normal IPL: Bypass TTYPE#5");
+        }
+
+        // TOD is cleared for this node; if it has children, start clearing their registers
+        for (std::list<tod_topology_node*>::iterator child = (i_tod_node->i_children).begin();
+             child != (i_tod_node->i_children).end();
+             ++child)
+        {
+            tod_topology_node* tod_node = *child;
+            FAPI_TRY(clear_tod_node(tod_node, i_tod_sel, i_is_mpipl), "Failure clearing downstream TOD node!");
+        }
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode configure_tod_node(tod_topology_node*           i_tod_node,
+                                         const p9_tod_setup_tod_sel i_tod_sel,
+                                         const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        char l_targetStr[fapi2::MAX_ECMD_STRING_LEN];
+
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+
+        fapi2::toString(target, l_targetStr, fapi2::MAX_ECMD_STRING_LEN);
+        FAPI_DBG("Start: Configuring %s", l_targetStr);
+
+        const bool is_mdmt = (i_tod_node->i_tod_master && i_tod_node->i_drawer_master);
+
+        FAPI_TRY(configure_pss_mss_ctrl_reg(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring pss_mss_ctrl_reg");
+
+        if (!is_mdmt)
+        {
+            FAPI_TRY(configure_s_path_ctrl_reg(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring s_path_ctrl_reg");
+        }
+
+        FAPI_TRY(configure_port_ctrl_regs(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring port_ctrl_regs");
+
+        if (is_mdmt)
+        {
+            FAPI_TRY(configure_m_path_ctrl_reg(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring m_path_ctrl_reg");
+        }
+
+        FAPI_TRY(configure_i_path_ctrl_reg(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring i_path_ctrl_reg");
+
+        FAPI_TRY(init_chip_ctrl_reg(i_tod_node, i_tod_sel, i_osc_sel), "Failure configuring init_chip_ctrl_reg");
+
+        // TOD is configured for this node; if it has children, start their configuration
+        for (std::list<tod_topology_node*>::iterator child = (i_tod_node->i_children).begin();
+             child != (i_tod_node->i_children).end();
+             ++child)
+        {
+            tod_topology_node* tod_node = *child;
+            FAPI_TRY(configure_tod_node(tod_node, i_tod_sel, i_osc_sel),
+                     "Failure configuring downstream node!");
+        }
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    fapi2::ReturnCode configure_pss_mss_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        fapi2::buffer<uint64_t> data(0x0);
+
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+
+        FAPI_DBG("Start");
+
+        const bool is_mdmt = (i_tod_node->i_tod_master && i_tod_node->i_drawer_master);
+
+        // Read PERV_TOD_PSS_MSS_CTRL_REG in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, PERV_TOD_PSS_MSS_CTRL_REG, data),
+                 "Error from fapiGetScom when retrieving PERV_TOD_PSS_MSS_CTRL_REG!");
+        FAPI_DBG("Set Master TOD/Slave TOD and Master Drawer/Slave Drawer");
+
+        if (i_tod_sel == TOD_PRIMARY)
+        {
+            if (is_mdmt)
+            {
+                data.setBit<PERV_TOD_PSS_MSS_CTRL_REG_PRI_M_S_SELECT>();
+
+                if (i_osc_sel == TOD_OSC_0             ||
+                    i_osc_sel == TOD_OSC_0_AND_1       ||
+                    i_osc_sel == TOD_OSC_0_AND_1_SEL_0)
+                {
+                    data.clearBit<PERV_TOD_PSS_MSS_CTRL_REG_PRI_M_PATH_SELECT>();
+                }
+                else if (i_osc_sel == TOD_OSC_1        ||
+                         i_osc_sel == TOD_OSC_0_AND_1_SEL_1)
+                {
+                    data.setBit<PERV_TOD_PSS_MSS_CTRL_REG_PRI_M_PATH_SELECT>();
+                }
+
+                FAPI_ASSERT(i_osc_sel != TOD_OSC_NONE, fapi2::P9_TOD_SETUP_INVALID_TOPOLOGY().set_TARGET(target).set_OSCSEL(i_osc_sel),
+                            "Invalid oscillator configuration!");
+            }
+            else // Slave nodes (Drawer master is still a slave)
+            {
+                data.clearBit<PERV_TOD_PSS_MSS_CTRL_REG_PRI_M_S_SELECT>();
+            }
+
+            if (i_tod_node->i_drawer_master)
+            {
+                data.setBit<PERV_TOD_PSS_MSS_CTRL_REG_PRI_M_S_DRAWER_SELECT>();
+            }
+        }
+        else // (i_tod_sel==TOD_SECONDARY)
+        {
+            if (is_mdmt)
+            {
+                data.setBit<PERV_TOD_PSS_MSS_CTRL_REG_SEC_M_S_SELECT>();
+
+                if (i_osc_sel == TOD_OSC_1       ||
+                    i_osc_sel == TOD_OSC_0_AND_1 ||
+                    i_osc_sel == TOD_OSC_0_AND_1_SEL_1)
+                {
+                    data.setBit<PERV_TOD_PSS_MSS_CTRL_REG_SEC_M_PATH_SELECT>();
+                }
+                else if (i_osc_sel == TOD_OSC_0  ||
+                         i_osc_sel == TOD_OSC_0_AND_1_SEL_0)
+                {
+                    data.clearBit<PERV_TOD_PSS_MSS_CTRL_REG_SEC_M_PATH_SELECT>();
+                }
+
+                FAPI_ASSERT(i_osc_sel != TOD_OSC_NONE, fapi2::P9_TOD_SETUP_INVALID_TOPOLOGY().set_TARGET(target).set_OSCSEL(i_osc_sel),
+                            "Invalid oscillator configuration!");
+            }
+            else // Slave nodes (Drawer master is still a slave)
+            {
+                data.clearBit<PERV_TOD_PSS_MSS_CTRL_REG_SEC_M_S_SELECT>();
+            }
+
+            if (i_tod_node->i_drawer_master)
+            {
+                data.setBit<PERV_TOD_PSS_MSS_CTRL_REG_SEC_M_S_DRAWER_SELECT>();
+            }
+        }
+
+        FAPI_TRY(fapi2::putScom(*target, PERV_TOD_PSS_MSS_CTRL_REG, data),
+                 "fapiPutScom error for PERV_TOD_PSS_MSS_CTRL_REG SCOM.");
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    fapi2::ReturnCode configure_s_path_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        fapi2::buffer<uint64_t> data(0x0);
+
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+
+        // Read PERV_TOD_S_PATH_CTRL_REG in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, PERV_TOD_S_PATH_CTRL_REG, data),
+                 "Error from fapiGetScom when retrieving PERV_TOD_S_PATH_CTRL_REG!");
+
+        // Slave TODs are enabled on all but the MDMT
+        FAPI_DBG("Selection of Slave OSC path");
+
+        if (i_tod_sel == TOD_PRIMARY)
+        {
+            data.clearBit<PERV_TOD_S_PATH_CTRL_REG_PRI_SELECT>(); // For primary slave, use slave path 0 (path_0_sel=OFF)
+
+            // Set CPS deviation to 75% (CPS deviation bits = 0xC, factor=1), 8 valid steps to enable step check
+            data.insertFromRight<PERV_TOD_S_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR,
+                                 PERV_TOD_S_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR_LEN>(STEP_CHECK_CPS_DEVIATION_FACTOR_1);
+            data.insertFromRight<PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_CPS_DEVIATION,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_CPS_DEVIATION_LEN>(STEP_CHECK_CPS_DEVIATION_75_00_PCENT);
+            data.insertFromRight<PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT_LEN>(STEP_CHECK_VALIDITY_COUNT_8);
+        }
+        else // (i_tod_sel==TOD_SECONDARY)
+        {
+            data.setBit<PERV_TOD_S_PATH_CTRL_REG_SEC_SELECT>();   // For secondary slave, use slave path 1 (path_1_sel=ON)
+
+            // Set CPS deviation to 75% (CPS deviation bits = 0xC, factor=1), 8 valid steps to enable step check
+            data.insertFromRight<PERV_TOD_S_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR,
+                                 PERV_TOD_S_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR_LEN>(STEP_CHECK_CPS_DEVIATION_FACTOR_1);
+            data.insertFromRight<PERV_TOD_S_PATH_CTRL_REG_1_STEP_CHECK_CPS_DEVIATION,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_CPS_DEVIATION_LEN>(STEP_CHECK_CPS_DEVIATION_75_00_PCENT);
+            data.insertFromRight<PERV_TOD_S_PATH_CTRL_REG_1_STEP_CHECK_VALIDITY_COUNT,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT_LEN>(STEP_CHECK_VALIDITY_COUNT_8);
+        }
+
+        FAPI_TRY(fapi2::putScom(*target, PERV_TOD_S_PATH_CTRL_REG, data),
+                 "fapiPutScom error for PERV_TOD_S_PATH_CTRL_REG SCOM.");
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    fapi2::ReturnCode configure_port_ctrl_regs(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+        fapi2::buffer<uint64_t> data(0x0);
+        fapi2::buffer<uint64_t> data2(0x0);
+        uint32_t port_ctrl_reg = 0;
+        uint32_t port_ctrl_check_reg = 0;
+        uint32_t port_rx_select_val = 0;
+        uint32_t path_sel = 0;
+
+        const bool is_mdmt = (i_tod_node->i_tod_master && i_tod_node->i_drawer_master);
+
+        // PERV_TOD_PRI_PORT_0_CTRL_REG is only used for Primary configurations
+        // PERV_TOD_SEC_PORT_1_CTRL_REG is only used for Secondary configurations
+        // In order to check primary and secondary networks are working simultaneously...
+        //  - The result of PERV_TOD_PRI_PORT_0_CTRL_REG are also inserted into PERV_TOD_SEC_PORT_0_CTRL_REG
+        //    (preserving i_path_delay which can be different between 40001 and 40003)
+        //  - The result of PERV_TOD_SEC_PORT_1_CTRL_REG are also inserted into PERV_TOD_PRI_PORT_1_CTRL_REG
+        if (i_tod_sel == TOD_PRIMARY)
+        {
+            FAPI_DBG("PERV_TOD_PRI_PORT_0_CTRL_REG will be configured for primary topology");
+            port_ctrl_reg = PERV_TOD_PRI_PORT_0_CTRL_REG;
+            port_ctrl_check_reg = PERV_TOD_SEC_PORT_0_CTRL_REG;
+        }
+        else // (i_tod_sel==TOD_SECONDARY)
+        {
+            FAPI_DBG("PERV_TOD_SEC_PORT_1_CTRL_REG will be configured for secondary topology");
+            port_ctrl_reg = PERV_TOD_SEC_PORT_1_CTRL_REG;
+            port_ctrl_check_reg = PERV_TOD_PRI_PORT_1_CTRL_REG;
+        }
+
+        // Read port_ctrl_reg in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, port_ctrl_reg, data),
+                 "Error from fapiGetScom when retrieving port_ctrl_reg!");
+
+        // Read port_ctrl_check_reg in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, port_ctrl_check_reg, data2),
+                 "Error from fapiGetScom when retrieving port_ctrl_check_reg!");
+
+        //Determine RX port
+        switch (i_tod_node->i_bus_rx)
+        {
+            case(XBUS0):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X0_SEL;
+                break;
+
+            case(XBUS1):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X1_SEL;
+                break;
+
+            case(XBUS2):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X2_SEL;
+                break;
+
+            case(XBUS3):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X3_SEL;
+                break;
+
+            case(XBUS4):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X4_SEL;
+                break;
+
+            case(XBUS5):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X5_SEL;
+                break;
+
+            case(XBUS6):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X6_SEL;
+                break;
+
+            case(XBUS7):
+                port_rx_select_val = TOD_PORT_CTRL_REG_RX_X7_SEL;
+                break;
+
+            case(NONE):
+                break; //MDMT has no rx
+        }
+
+        data.insertFromRight<TOD_PORT_CTRL_REG_RX_SEL,
+                             TOD_PORT_CTRL_REG_RX_LEN>(port_rx_select_val);
+        data2.insertFromRight<TOD_PORT_CTRL_REG_RX_SEL,
+                              TOD_PORT_CTRL_REG_RX_LEN>(port_rx_select_val);
+
+        //Determine which tx path should be selected for all children
+        if (is_mdmt)
+        {
+            if (i_tod_sel == TOD_PRIMARY)
+            {
+                if (i_osc_sel == TOD_OSC_0       ||
+                    i_osc_sel == TOD_OSC_0_AND_1 ||
+                    i_osc_sel == TOD_OSC_0_AND_1_SEL_0)
+                {
+                    path_sel = TOD_PORT_CTRL_REG_M_PATH_0;
+                }
+                else if (i_osc_sel == TOD_OSC_1  ||
+                         i_osc_sel == TOD_OSC_0_AND_1_SEL_1)
+                {
+                    path_sel = TOD_PORT_CTRL_REG_M_PATH_1;
+                }
+
+                FAPI_ASSERT(i_osc_sel != TOD_OSC_NONE, fapi2::P9_TOD_SETUP_INVALID_TOPOLOGY().set_TARGET(target).set_OSCSEL(i_osc_sel),
+                            "Invalid oscillator configuration!");
+            }
+            else // i_tod_sel==TOD_SECONDARY
+            {
+                if (i_osc_sel == TOD_OSC_1       ||
+                    i_osc_sel == TOD_OSC_0_AND_1 ||
+                    i_osc_sel == TOD_OSC_0_AND_1_SEL_1)
+                {
+                    path_sel = TOD_PORT_CTRL_REG_M_PATH_1;
+                }
+                else if (i_osc_sel == TOD_OSC_0  ||
+                         i_osc_sel == TOD_OSC_0_AND_1_SEL_0)
+                {
+                    path_sel = TOD_PORT_CTRL_REG_M_PATH_0;
+                }
+
+                FAPI_ASSERT(i_osc_sel != TOD_OSC_NONE, fapi2::P9_TOD_SETUP_INVALID_TOPOLOGY().set_TARGET(target).set_OSCSEL(i_osc_sel),
+                            "Invalid oscillator configuration!");
+            }
+        }
+        else // Chip is not master; slave path selected
+        {
+            if (i_tod_sel == TOD_PRIMARY)
+            {
+                path_sel = TOD_PORT_CTRL_REG_S_PATH_0;
+            }
+            else // (i_tod_sel==TOD_SECONDARY)
+            {
+                path_sel = TOD_PORT_CTRL_REG_S_PATH_1;
+            }
+        }
+
+        for (std::list<tod_topology_node*>::iterator child = (i_tod_node->i_children).begin();
+             child != (i_tod_node->i_children).end();
+             ++child)
+        {
+            // Loop through all of the out busses, determine which tx buses to enable as senders
+            tod_topology_node* tod_node = *child;
+
+            switch (tod_node->i_bus_tx)
+            {
+                case(XBUS0):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X0_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X0_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X0_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X0_EN>();
+                    break;
+
+                case(XBUS1):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X1_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X1_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X1_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X1_EN>();
+                    break;
+
+                case(XBUS2):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X2_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X2_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X2_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X2_EN>();
+                    break;
+
+                case(XBUS3):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X3_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X3_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X3_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X3_EN>();
+                    break;
+
+                case(XBUS4):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X4_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X4_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X4_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X4_EN>();
+                    break;
+
+                case(XBUS5):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X5_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X5_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X5_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X5_EN>();
+                    break;
+
+                case(XBUS6):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X6_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X6_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X6_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X6_EN>();
+                    break;
+
+                case(XBUS7):
+                    data.insertFromRight<TOD_PORT_CTRL_REG_TX_X7_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data.setBit<TOD_PORT_CTRL_REG_TX_X7_EN>();
+                    data2.insertFromRight<TOD_PORT_CTRL_REG_TX_X7_SEL, TOD_PORT_CTRL_REG_TX_LEN>(path_sel);
+                    data2.setBit<TOD_PORT_CTRL_REG_TX_X7_EN>();
+                    break;
+
+                case(NONE):
+                    FAPI_ASSERT((tod_node->i_bus_tx != NONE),
+                                fapi2::P9_TOD_SETUP_INVALID_TOPOLOGY().set_TARGET(target).set_OSCSEL(i_osc_sel), "i_tod_node->i_bus_rx is set to NONE");
+                    break;
+            }
+        }
+
+        // All children have been configured; save both port configurations!
+        FAPI_TRY(fapi2::putScom(*target, port_ctrl_reg, data), "fapiPutScom error for port_ctrl_reg SCOM.");
+        FAPI_TRY(fapi2::putScom(*target, port_ctrl_check_reg, data2),
+                 "fapiPutScom error for port_ctrl_check_reg SCOM.");
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    fapi2::ReturnCode configure_m_path_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        fapi2::buffer<uint64_t> data(0x0);
+
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+
+        // Read PERV_TOD_M_PATH_CTRL_REG in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, PERV_TOD_M_PATH_CTRL_REG, data),
+                 "Error from fapiGetScom when retrieving PERV_TOD_M_PATH_CTRL_REG!");
+
+        // Configure Master OSC0/OSC1 path
+        FAPI_DBG("Configuring Master OSC path in PERV_TOD_M_PATH_CTRL_REG");
+
+        if (i_osc_sel == TOD_OSC_0             ||
+            i_osc_sel == TOD_OSC_0_AND_1       ||
+            i_osc_sel == TOD_OSC_0_AND_1_SEL_0 ||
+            i_osc_sel == TOD_OSC_0_AND_1_SEL_1)
+        {
+            FAPI_DBG("OSC0 is valid; master path-0 will be configured.");
+
+            // OSC0 is connected
+            data.clearBit<PERV_TOD_M_PATH_CTRL_REG_0_OSC_NOT_VALID>();
 
+            // OSC0 step alignment enabled
+            data.clearBit<PERV_TOD_M_PATH_CTRL_REG_0_STEP_ALIGN_DISABLE>();
+
+            // Set 512 steps per sync for path 0
+            data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_SYNC_CREATE_SPS_SELECT,
+                                 PERV_TOD_M_PATH_CTRL_REG_SYNC_CREATE_SPS_SELECT_LEN>(TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_512);
+
+            // Set step check CPS deviation to 50%
+            data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_0_STEP_CHECK_CPS_DEVIATION,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_CPS_DEVIATION_LEN>(STEP_CHECK_CPS_DEVIATION_50_00_PCENT);
+
+            // 8 valid steps are required before step check is enabled
+            data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT_LEN>(STEP_CHECK_VALIDITY_COUNT_8);
+        }
+        else
+        {
+            FAPI_DBG("OSC0 is not connected.");
+
+            // OSC0 is not connected; any previous path-0 settings will be ignored
+            data.setBit<PERV_TOD_M_PATH_CTRL_REG_0_OSC_NOT_VALID>();
+        }
+
+        if (i_osc_sel == TOD_OSC_1             ||
+            i_osc_sel == TOD_OSC_0_AND_1       ||
+            i_osc_sel == TOD_OSC_0_AND_1_SEL_0 ||
+            i_osc_sel == TOD_OSC_0_AND_1_SEL_1)
+        {
+            FAPI_DBG("OSC1 is valid; master path-1 will be configured.");
+
+            // OSC1 is connected
+            data.clearBit<PERV_TOD_M_PATH_CTRL_REG_1_OSC_NOT_VALID>();
+
+            // OSC1 step alignment enabled
+            data.clearBit<PERV_TOD_M_PATH_CTRL_REG_1_STEP_ALIGN_DISABLE>();
+
+            // Set 512 steps per sync for path 1
+            data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_SYNC_CREATE_SPS_SELECT,
+                                 PERV_TOD_M_PATH_CTRL_REG_SYNC_CREATE_SPS_SELECT_LEN>(TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_512);
+
+            // Set step check CPS deviation to 50%
+            data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_1_STEP_CHECK_CPS_DEVIATION,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_CPS_DEVIATION_LEN>(STEP_CHECK_CPS_DEVIATION_50_00_PCENT);
+
+            // 8 valid steps are required before step check is enabled
+            data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_1_STEP_CHECK_VALIDITY_COUNT,
+                                 PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT_LEN>(STEP_CHECK_VALIDITY_COUNT_8);
+        }
+        else
+        {
+            FAPI_DBG("OSC1 is not connected.");
+
+            // OSC1 is not connected; any previous path-1 settings will be ignored
+            data.setBit<PERV_TOD_M_PATH_CTRL_REG_1_OSC_NOT_VALID>();
+        }
+
+        // CPS deviation factor configures both path-0 and path-1
+        data.insertFromRight<PERV_TOD_M_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR,
+                             PERV_TOD_S_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR_LEN>(STEP_CHECK_CPS_DEVIATION_FACTOR_1);
+
+        FAPI_TRY(fapi2::putScom(*target, PERV_TOD_M_PATH_CTRL_REG, data),
+                 "fapiPutScom error for PERV_TOD_M_PATH_CTRL_REG SCOM.");
+
+
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    fapi2::ReturnCode configure_i_path_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        fapi2::buffer<uint64_t> data(0x0);
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+        uint32_t port_ctrl_reg = 0;
+
+        FAPI_DBG("Setting internal path delay");
+
+        // This is the number of TOD-grid-cycles to delay the internal path (0-0xFF valid); 1 TOD-grid-cycle = 400ps (default)
+        if (i_tod_sel == TOD_PRIMARY)
+        {
+            // Primary topology internal path delay set PERV_TOD_PRI_PORT_0_CTRL_REG, regardless of master/slave/port
+            FAPI_DBG("PERV_TOD_PRI_PORT_0_CTRL_REG will be used to set internal delay");
+            port_ctrl_reg = PERV_TOD_PRI_PORT_0_CTRL_REG;
+        }
+        else // (i_tod_sel==TOD_SECONDARY)
+        {
+            // Secondary topology internal path delay set PERV_TOD_SEC_PORT_0_CTRL_REG regardless of master/slave/port
+            FAPI_DBG("PERV_TOD_SEC_PORT_0_CTRL_REG will be used to set internal delay");
+            port_ctrl_reg = PERV_TOD_SEC_PORT_0_CTRL_REG;
+        }
+
+        FAPI_TRY(fapi2::getScom(*target, port_ctrl_reg, data),
+                 "Error from fapiGetScom when retrieving port_ctrl_reg!");
+        FAPI_DBG("configuring an internal delay of %d TOD-grid-cycles", i_tod_node->o_int_path_delay);
+        data.insertFromRight<TOD_PORT_CTRL_REG_I_PATH_DELAY,
+                             TOD_PORT_CTRL_REG_I_PATH_DELAY_LEN>(i_tod_node->o_int_path_delay);
+        FAPI_TRY(fapi2::putScom(*target, port_ctrl_reg, data), "fapiPutScom error for port_ctrl_reg SCOM.");
+
+        FAPI_DBG("Enable delay logic in PERV_TOD_I_PATH_CTRL_REG");
+        // Read PERV_TOD_I_PATH_CTRL_REG in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, PERV_TOD_I_PATH_CTRL_REG, data),
+                 "Error from fapiGetScom when retrieving PERV_TOD_I_PATH_CTRL_REG!");
+
+        // Ensure delay is enabled
+        data.clearBit<PERV_TOD_I_PATH_CTRL_REG_DELAY_DISABLE>();
+        data.clearBit<PERV_TOD_I_PATH_CTRL_REG_DELAY_ADJUST_DISABLE>();
+
+        // Deviation for internal OSC should be set to max, allowing backup master TOD to run the active topology,
+        // when switching from Slave OSC path to Master OSC path
+        data.insertFromRight<PERV_TOD_I_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION,
+                             PERV_TOD_I_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_LEN>(STEP_CHECK_CPS_DEVIATION_93_75_PCENT);
+        data.insertFromRight<PERV_TOD_I_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR,
+                             PERV_TOD_I_PATH_CTRL_REG_STEP_CHECK_CPS_DEVIATION_FACTOR_LEN>(STEP_CHECK_CPS_DEVIATION_FACTOR_1);
+        data.insertFromRight<PERV_TOD_I_PATH_CTRL_REG_STEP_CHECK_VALIDITY_COUNT,
+                             PERV_TOD_S_PATH_CTRL_REG_0_STEP_CHECK_VALIDITY_COUNT_LEN>(STEP_CHECK_VALIDITY_COUNT_8);
+        FAPI_TRY(fapi2::putScom(*target, PERV_TOD_I_PATH_CTRL_REG, data),
+                 "fapiPutScom error for PERV_TOD_I_PATH_CTRL_REG SCOM.");
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    void display_tod_nodes(const tod_topology_node* i_tod_node, const uint32_t i_depth)
+    {
+        char l_targetStr[fapi2::MAX_ECMD_STRING_LEN];
+
+        if (i_tod_node == NULL || i_tod_node->i_target == NULL)
+        {
+            FAPI_ERR("NULL tod_node or target passed to display_tod_nodes()!");
+        }
+
+
+        fapi2::toString(i_tod_node->i_target, l_targetStr, fapi2::MAX_ECMD_STRING_LEN);
+        FAPI_INF("%s (Delay = %d)", l_targetStr, i_tod_node->o_int_path_delay);
+
+        for (std::list<tod_topology_node*>::const_iterator child = (i_tod_node->i_children).begin();
+             child != (i_tod_node->i_children).end();
+             ++child)
+        {
+            display_tod_nodes(*child, i_depth + 1);
+        }
+    }
+
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode calculate_node_delays(tod_topology_node* i_tod_node)
+    {
+        const fapi2::Target<fapi2::TARGET_TYPE_SYSTEM> FAPI_SYSTEM;
+        fapi2::buffer<uint64_t> data(0x0);
+        uint32_t longest_delay = 0;
+        uint32_t freq_x = 0;
+        uint32_t freq_a = 0;
+
+
+        FAPI_DBG("Start");
+        // retrieve X-bus and A-bus frequencies
+        FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_FREQ_X, FAPI_SYSTEM, freq_x),
+                 "Failure reading XBUS frequency attribute!");
+        FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_FREQ_A, FAPI_SYSTEM, freq_a),
+                 "Failure reading ABUS frequency attribute!");
+
+        // Bus frequencies are global for the system (i.e. A0 and A1 will always run with the same frequency)
+        FAPI_DBG("XBUS=%dMHz ABUS=%dMHz", freq_x, freq_a);
+
+        // Find the most-delayed path in the topology; this is the MDMT's delay
+        FAPI_TRY(calculate_longest_topolopy_delay(i_tod_node, freq_x, freq_a, longest_delay),
+                 "Failure calculating longest topology delay!");
+        FAPI_DBG("the longest delay is %d TOD-grid-cycles.", longest_delay);
+
+        FAPI_TRY(set_topology_delays(i_tod_node, freq_x, freq_a, longest_delay),
+                 "Unable to set topology delays!");
+
+        // Finally, the MDMT delay must include additional TOD-grid-cycles to account for staging latches in slaves
+        i_tod_node->o_int_path_delay += MDMT_TOD_GRID_CYCLE_STAGING_DELAY;
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode calculate_longest_topolopy_delay(tod_topology_node* i_tod_node,
+            const uint32_t     i_freq_x,
+            const uint32_t     i_freq_a,
+            uint32_t&          o_longest_delay)
+    {
+        uint32_t node_delay = 0;
+        uint32_t current_longest_delay = 0;
+
+        FAPI_DBG("Start");
+
+        FAPI_TRY(calculate_node_link_delay(i_tod_node, i_freq_x, i_freq_a, node_delay),
+                 "Failure calculating single node delay!");
+        o_longest_delay = node_delay;
+
+        for (std::list<tod_topology_node*>::const_iterator child = (i_tod_node->i_children).begin();
+             child != (i_tod_node->i_children).end();
+             ++child)
+        {
+            tod_topology_node* tod_node = *child;
+            FAPI_TRY(calculate_longest_topolopy_delay(tod_node, i_freq_x, i_freq_a, node_delay),
+                     "Failure calculating topology delay!");
+
+            if (node_delay > current_longest_delay)
+            {
+                current_longest_delay = node_delay;
+            }
+        }
+
+        o_longest_delay += current_longest_delay;
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode calculate_node_link_delay(tod_topology_node* i_tod_node,
+            const uint32_t     i_freq_x,
+            const uint32_t     i_freq_a,
+            uint32_t&          o_node_delay)
+    {
+        fapi2::buffer<uint64_t> data(0x0);
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+
+        FAPI_DBG("Start");
+
+        // MDMT is originator and therefore has no node delay
+        if (i_tod_node->i_tod_master && i_tod_node->i_drawer_master)
+        {
+            o_node_delay = 0;
+        }
+        else // slave node
+        {
+            uint32_t bus_mode_addr = 0;
+            //uint32_t bus_mode_sel = 0;
+            uint32_t bus_freq = 0;
+            uint8_t  bus_delay = 0;
+
+            switch (i_tod_node->i_bus_rx)
+            {
+                case(XBUS0):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X0_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS1):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X1_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS2):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X2_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS3):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X3_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS4):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X4_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS5):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X5_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS6):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X6_ROUND_TRIP_DELAY;*/ break;
+
+                case(XBUS7):
+                    bus_freq = i_freq_x; /*bus_mode_addr = PB_X_MODE_0x04010C0A; bus_mode_sel = PB_X_MODE_LINK_X7_ROUND_TRIP_DELAY;*/ break;
+
+                case(NONE):
+                    FAPI_ASSERT((i_tod_node->i_bus_rx != NONE),
+                                fapi2::P9_TOD_SETUP_INVALID_TOPOLOGY().set_TARGET(target).set_OSCSEL(i_freq_x), "i_tod_node->i_bus_rx is set to NONE");
+                    break;
+            }
+
+            FAPI_TRY(fapi2::getScom(*target, bus_mode_addr, data),
+                     "Error from fapiGetScom when retrieving bus_mode_addr!");
+            //TODO Figure out where LINK_ROUND_TRIP_DELAY_LEN is coming from data.extract(&bus_delay, bus_mode_sel, LINK_ROUND_TRIP_DELAY_LEN);
+
+            // By default, the TOD grid runs at 400ps; TOD counts its delay based on this
+            // Example: Bus round trip delay is 35 cycles and the bus is running at 4800MHz
+            //            - Divide by 2 to get one-way delay time
+            //            - Divide by 4800 * 10^6 to get delay in seconds
+            //            - Multiply by 10^12 to get delay in picoseconds
+            //            - Divide by 400ps to get TOD-grid-cycles
+            //            - (To avoid including math.h) Add 1 and cast to uint32_t to round up to nearest TOD-grid-cycle
+            //            - (To avoid including math.h) 10^12/10^6=1000000
+            //            - (uint32_t)((        35        / 2 /        (4800      * 10^6) * 10^12 / 400        ) + 1) = 10 TOD-grid-cycles
+            o_node_delay = (uint32_t)(((double)bus_delay / 2 / (double)bus_freq  * 1000000       / TOD_GRID_PS) + 1);
+        }
+
+        // This is not the final internal path delay, only saved so two calls aren't needed to calculate_node_link_delay
+        i_tod_node->o_int_path_delay = o_node_delay;
+
+        FAPI_DBG("TOD-grid-cycles for single link: %d", o_node_delay);
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode set_topology_delays(tod_topology_node* i_tod_node,
+                                          const uint32_t     i_freq_x,
+                                          const uint32_t     i_freq_a,
+                                          const uint32_t     i_longest_delay)
+    {
+        FAPI_DBG("Start");
+
+        // Retrieve saved node_delay from calculate_node_link_delay instead of making a second call
+        i_tod_node->o_int_path_delay = i_longest_delay - (i_tod_node->o_int_path_delay);
+
+        // Verify the delay is between 0 and 255 inclusive.
+        FAPI_ASSERT((i_tod_node->o_int_path_delay >= MIN_TOD_DELAY &&
+                     i_tod_node->o_int_path_delay <= MAX_TOD_DELAY),
+                    fapi2::P9_TOD_SETUP_INVALID_NODE_DELAY().set_TARGET(i_tod_node->i_target).set_DELAY(i_tod_node->o_int_path_delay),
+                    "Invalid delay of %d calculated!");
+
+        // Recurse on downstream nodes
+        for (std::list<tod_topology_node*>::const_iterator child = (i_tod_node->i_children).begin();
+             child != (i_tod_node->i_children).end();
+             ++child)
+        {
+            tod_topology_node* tod_node = *child;
+            FAPI_TRY(set_topology_delays(tod_node, i_freq_x, i_freq_a, i_tod_node->o_int_path_delay),
+                     "Failure calculating topology delay!");
+        }
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+    //---------------------------------------------------------------------------------
+    // NOTE: description in header
+    //---------------------------------------------------------------------------------
+    fapi2::ReturnCode init_chip_ctrl_reg (tod_topology_node*           i_tod_node,
+                                          const p9_tod_setup_tod_sel i_tod_sel,
+                                          const p9_tod_setup_osc_sel i_osc_sel)
+    {
+        fapi2::buffer<uint64_t> chic_ctrlReg_val(0x0);
+
+        fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>* target = i_tod_node->i_target;
+
+        FAPI_DBG("Start");
+
+        FAPI_DBG("Set low order step value to 3F in PERV_TOD_CHIP_CTRL_REG");
+        // Read PERV_TOD_CHIP_CTRL_REG in order to perserve any prior configuration
+        FAPI_TRY(fapi2::getScom(*target, PERV_TOD_CHIP_CTRL_REG, chic_ctrlReg_val),
+                 "Error from fapiGetScom when retrieving PERV_TOD_CHIP_CTRL_REG!");
+
+        // Default core sync period is 8us
+        chic_ctrlReg_val.insertFromRight<PERV_TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SELECT,
+                                         PERV_TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SELECT_LEN>(TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_8);
+
+        // Enable internal path sync check
+        chic_ctrlReg_val.clearBit<PERV_TOD_CHIP_CTRL_REG_I_PATH_SYNC_CHECK_DISABLE>();
+
+        // 1x sync boundaries for Move-TOD-To-Timebase
+        chic_ctrlReg_val.clearBit<PERV_TOD_CHIP_CTRL_REG_MOVE_TO_TB_ON_2X_SYNC_ENABLE>();
+
+        // Use eclipz sync mechanism
+        chic_ctrlReg_val.clearBit<PERV_TOD_CHIP_CTRL_REG_USE_TB_SYNC_MECHANISM>();
+
+        // Use timebase step sync from internal path
+        chic_ctrlReg_val.clearBit<PERV_TOD_CHIP_CTRL_REG_USE_TB_STEP_SYNC>();
+
+        // Chip TOD WOF incrementer ratio (eclipz mode)
+        // 4-bit WOF counter is incremented with each 200MHz clock cycle
+        chic_ctrlReg_val.insertFromRight<PERV_TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VALUE,
+                                         PERV_TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VALUE_LEN>(TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VAL_3F);
+
+        // Stop TOD when system checkstop occurs
+        chic_ctrlReg_val.clearBit<PERV_TOD_CHIP_CTRL_REG_XSTOP_GATE>();
+
+        FAPI_TRY(fapi2::putScom(*target, PERV_TOD_CHIP_CTRL_REG, chic_ctrlReg_val),
+                 "fapiPutScom error for PERV_TOD_CHIP_CTRL_REG SCOM.");
+
+    fapi_try_exit:
+        FAPI_DBG("Exiting...");
+        return fapi2::current_err;
+    }
+
+} // extern "C"
diff --git a/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.H b/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.H
index 2bf76396d..3daa49182 100644
--- a/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.H
+++ b/src/import/chips/p9/procedures/hwp/nest/p9_tod_setup.H
@@ -30,15 +30,9 @@
 // *HWP HWP Owner: Christina Graves clgraves@us.ibm.com
 // *HWP FW Owner: Thi Tran thi@us.ibm.com
 // *HWP Team: Nest
-// *HWP Level: 1
-// *HWP Consumed by:
+// *HWP Level: 2
+// *HWP Consumed by: SBE
 // ----------------------------------------------------------------------------------
-//
-// *! ADDITIONAL COMMENTS :
-// *!
-// *!
-// *!
-//-----------------------------------------------------------------------------------
 
 #ifndef _P9_TOD_SETUP_H_
 #define _P9_TOD_SETUP_H_
@@ -48,7 +42,7 @@
 //-----------------------------------------------------------------------------------
 
 #include <fapi2.H>
-#include "p9_tod_utils.H"
+#include <p9_tod_utils.H>
 
 //-----------------------------------------------------------------------------------
 // Structure definitions
@@ -75,15 +69,141 @@ extern "C" {
 // Function prototype
 //-----------------------------------------------------------------------------------
 
-/// @brief Gets the value in the TFMR register
-/// @param[in] i_target       => P9 chip target
-/// @param[out] o_tfmr_val    => Value that is in the TFMR register
-/// @return FAPI_RC_SUCCESS if TFMR read is successful else FAPI or ECMD error is sent through
+/// @brief Setup the tod
+/// @param[in] i_tod_node  Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel   Specifies the topology to configure
+/// @param[in] i_osc_sel   Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if TOD topology is successfully configured else FAPI or ECMD error is sent through
     fapi2::ReturnCode p9_tod_setup(
         tod_topology_node* i_tod_node,
         const p9_tod_setup_tod_sel i_tod_sel,
         const p9_tod_setup_osc_sel i_osc_sel);
 
+/// @brief Configure the tod topology set up the oscilator select and the primary or secondary tod
+/// @param[in] i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if TOD topology is successfully configured
+///          else FAPI or ECMD error is sent through
+    fapi2::ReturnCode configure_tod_node(tod_topology_node*           i_tod_node,
+                                         const p9_tod_setup_tod_sel i_tod_sel,
+                                         const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Clear any previous topology that may have been set
+/// @param[in] i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to clear
+/// @param[in] i_is_mpipl => if this IPL is an MPIPL, additional setup is needed; determined via an attribute
+/// @return FAPI_RC_SUCCESS if TOD topology is successfully cleared
+///          else FAPI or ECMD error is sent through
+    fapi2::ReturnCode clear_tod_node(tod_topology_node*             i_tod_node,
+                                     const p9_tod_setup_tod_sel   i_tod_sel,
+                                     const fapi2::ATTR_IS_MPIPL_Type i_is_mpipl);
+
+/// @brief Configures the PSS_MSS_CTRL_REG; will be called by configure_tod_node
+/// @param[in]i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if the PSS_MSS_CTRL_REG is successfully configured
+///         else FAPI or ECMD error is sent through
+    fapi2::ReturnCode configure_pss_mss_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Configures the S_PATH_CTRL_REG; will be called by configure_tod_node
+/// @param[in]i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if the S_PATH_CTRL_REG is successfully configured
+///         else FAPI or ECMD error is sent through
+    fapi2::ReturnCode configure_s_path_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Configures the PORT_CTRL_REG; will be called by configure_tod_node
+/// @param[in]i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if the PORT_CTRL_REG is successfully configured
+///         else FAPI or ECMD error is sent through
+    fapi2::ReturnCode configure_port_ctrl_regs(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Configures the M_PATH_CTRL_REG; will be called by configure_tod_node
+/// @param[in]i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if the M_PATH_CTRL_REG is successfully configured
+///         else FAPI or ECMD error is sent through
+    fapi2::ReturnCode configure_m_path_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Configures the I_PATH_CTRL_REG; will be called by configure_tod_node
+/// @param[in]i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if the I_PATH_CTRL_REG is successfully configured
+///         else FAPI or ECMD error is sent through
+    fapi2::ReturnCode configure_i_path_ctrl_reg(tod_topology_node*           i_tod_node,
+            const p9_tod_setup_tod_sel i_tod_sel,
+            const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Configures the INIT_CHIP_CTRL_REG; will be called by configure_tod_node
+/// @param[in]i_tod_node => Reference to TOD topology (FAPI targets included within)
+/// @param[in] i_tod_sel => Specifies the topology to configure
+/// @param[in] i_osc_sel => Specifies the oscillator to use for the master
+/// @return FAPI_RC_SUCCESS if the INIT_CHIP_CTRL_REG is successfully configured
+///         else FAPI or ECMD error is sent through
+    fapi2::ReturnCode init_chip_ctrl_reg(tod_topology_node*           i_tod_node,
+                                         const p9_tod_setup_tod_sel i_tod_sel,
+                                         const p9_tod_setup_osc_sel i_osc_sel);
+
+/// @brief Displays the TOD topology
+/// @param[in] i_tod_node => Reference to TOD topology
+/// @param[in] depth => depth of children into the tod node that we are in
+/// @return void
+    void display_tod_nodes(const tod_topology_node* i_tod_node, const uint32_t depth);
+
+/// @brief Calculates and populates the topology delays
+/// @param[in] i_tod_node => Reference to TOD topology
+/// @return FAPI_RC_SUCCESS if TOD topology is successfully configured with delays else FAPI or ECMD error is sent through
+    fapi2::ReturnCode calculate_node_delays(tod_topology_node* i_tod_node);
+
+/// @brief Finds the longest delay in the topology (additionally sets each node delay)
+/// @param[in]  i_tod_node      => Reference to TOD topology
+/// @param[in]  i_freq_x        => XBUS frequency in MHz
+/// @param[in]  i_freq_a        => ABUS frequency in MHz
+/// @param[out] o_longest_delay => Longest delay in TOD-grid-cycles
+/// @return FAPI_RC_SUCCESS if a longest TOD delay was found in topology else FAPI or ECMD error is sent through
+    fapi2::ReturnCode calculate_longest_topolopy_delay(tod_topology_node* i_tod_node,
+            const uint32_t     i_freq_x,
+            const uint32_t     i_freq_a,
+            uint32_t&          o_longest_delay);
+
+/// @brief Calculates the delay for a node in TOD-grid-cycles
+/// @param[in]  i_tod_node   => Reference to TOD topology
+/// @param[in]  i_freq_x     => XBUS frequency in MHz
+/// @param[in]  i_freq_a     => ABUS frequency in MHz
+/// @param[out] o_node_delay => Delay of a single node in TOD-grid-cycles
+/// @return FAPI_RC_SUCCESS if TOD node delay is successfully calculated else FAPI or ECMD error is sent through
+    fapi2::ReturnCode calculate_node_link_delay(tod_topology_node* i_tod_node,
+            const uint32_t     i_freq_x,
+            const uint32_t     i_freq_a,
+            uint32_t&          o_node_delay);
+
+/// @brief Updates the topology struct with the final delay values
+/// @param[in]  i_tod_node      => Reference to TOD topology
+/// @param[in]  i_freq_x        => XBUS frequency in MHz
+/// @param[in]  i_freq_a        => ABUS frequency in MHz
+/// @param[in]  i_longest_delay => Longest delay in the topology
+/// @return FAPI_RC_SUCCESS if o_int_path_delay was set for every node in the topology else FAPI or ECMD error is sent through
+    fapi2::ReturnCode set_topology_delays(tod_topology_node* i_tod_node,
+                                          const uint32_t     i_freq_x,
+                                          const uint32_t     i_freq_a,
+                                          const uint32_t     i_longest_delay);
+
+
 } //extern "C"
 
 #endif //_P9_TOD_SETUP_H_
diff --git a/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.C b/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.C
index b78cc8dca..0560a5e5e 100644
--- a/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.C
+++ b/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.C
@@ -49,26 +49,152 @@ extern "C" {
 //--------------------------------------------------------------------------
     fapi2::ReturnCode p9_tod_utils_get_tfmr_reg(
         const fapi2::Target<fapi2::TARGET_TYPE_CORE>& i_target,
-        ecmdDataBufferBase& o_tfmr_val)
+        const uint8_t i_thread_num,
+        fapi2::buffer<uint64_t>& o_tfmr_val)
     {
         // mark HWP entry
         FAPI_DBG("Entering ...\n");
 
+        fapi2::buffer<uint64_t> data;
+        fapi2::buffer<uint64_t> data2;
 
-        /*fapi_try_exit:*/
+        //FAPI_DBG("proc_tod_utils_get_tfmr_reg: Setting SPR_MODE to thread number");
+        data.flush<0>().setBit<SPR_MODE_REG_MODE_SPRC_WR_EN>().setBit<SPR_MODE_REG_MODE_SPRC0_SEL>();
+        data2.flush<0>();
+
+        switch(i_thread_num)
+        {
+            case(0):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T0_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T0, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(1):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T1_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T1, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(2):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T2_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T2, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(3):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T3_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T3, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(4):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T4_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T4, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(5):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T5_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T5, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(6):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T6_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T6, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(7):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T7_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T7, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            default:
+                FAPI_ASSERT(true, fapi2::P9_INVALID_THREAD_NUM().set_TARGET(i_target).set_THREADNUMBER(i_thread_num),
+                            "Thread number error ");
+        }
+
+        FAPI_TRY(fapi2::putScom(i_target, C_SPR_MODE, data), "Error writing to EX_PERV_SPR_MODE");
+
+        FAPI_TRY(fapi2::putScom(i_target, C_SCOMC, data2), "Error writing to EX_PERV_L0_SCOM_SPRC");
+
+        //FAPI_DBG("proc_tod_utils_get_tfmr_reg: Reading SPRD for T0's TMFR");
+        //FAPI_TRY(fapi2::getScom(i_target, 0x2E010A81, o_tfmr_val), "Error getting EX_PERV_SPRD_L0");
+        FAPI_TRY(fapi2::getScom(i_target, 0x20010A81, o_tfmr_val), "Error getting EX_PERV_SPRD_L0");
+
+    fapi_try_exit:
         FAPI_DBG("Exiting...");
         return fapi2::current_err;
     }
 
     fapi2::ReturnCode p9_tod_utils_set_tfmr_reg(
         const fapi2::Target<fapi2::TARGET_TYPE_CORE>& i_target,
-        ecmdDataBufferBase& i_tfmr_val)
+        const uint8_t i_thread_num,
+        fapi2::buffer<uint64_t>& i_tfmr_val)
     {
         // mark HWP entry
         FAPI_DBG("Entering ...\n");
 
+        fapi2::buffer<uint64_t> data;
+        fapi2::buffer<uint64_t> data2;
+
+        //FAPI_DBG("proc_tod_utils_set_tfmr_reg: Setting SPR_MODE to thread number");
+        //FAPI_DBG("proc_tod_utils_set_tfmr_reg: Setting SPRC to thread's TMFR");
+        data.flush<0>().setBit<SPR_MODE_REG_MODE_SPRC_WR_EN>().setBit<SPR_MODE_REG_MODE_SPRC0_SEL>();
+        data2.flush<0>();
+
+        switch(i_thread_num)
+        {
+            case(0):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T0_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T0, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(1):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T1_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T1, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(2):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T2_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T2, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(3):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T3_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T3, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(4):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T4_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T4, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(5):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T5_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T5, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(6):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T6_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T6, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            case(7):
+                data.setBit<SPR_MODE_REG_MODE_SPRC_T7_SEL>();
+                data2.insertFromRight(SPRC_REG_SEL_TFMR_T7, SPRC_REG_SEL, SPRC_REG_SEL_LEN);
+                break;
+
+            default:
+                FAPI_ASSERT(true, fapi2::P9_INVALID_THREAD_NUM().set_TARGET(i_target).set_THREADNUMBER(i_thread_num),
+                            "Thread number error ");
+                break;
+        }
+
+        FAPI_TRY(fapi2::putScom(i_target, C_SPR_MODE, data), "Error doing putscom to EX_PERV_SPR_MODE");
+
+        FAPI_TRY(fapi2::putScom(i_target, C_SCOMC, data2), "Error doing putscom to EX_PERV_L0_SCOM_SPRC");
+
+        //FAPI_DBG("proc_tod_utils_set_tfmr_reg: Writing SPRD to set T0's TMFR");
+        //FAPI_TRY(fapi2::putScom(i_target, 0x2E010A81, i_tfmr_val), "Error doing putscom to EX_PERV_SPRD_L0");
+        FAPI_TRY(fapi2::putScom(i_target, 0x20010A81, i_tfmr_val), "Error doing putscom to EX_PERV_SPRD_L0");
 
-        /*fapi_try_exit:*/
+    fapi_try_exit:
         FAPI_DBG("Exiting...");
         return fapi2::current_err;
     }
diff --git a/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.H b/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.H
index 0aca1b485..cadc7bccb 100644
--- a/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.H
+++ b/src/import/chips/p9/procedures/hwp/nest/p9_tod_utils.H
@@ -30,8 +30,8 @@
 // *HWP HWP Owner: Christina Graves clgraves@us.ibm.com
 // *HWP FW Owner: Thi Tran thi@us.ibm.com
 // *HWP Team: Nest
-// *HWP Level: 1
-// *HWP Consumed by:
+// *HWP Level: 2
+// *HWP Consumed by: SBE
 // ----------------------------------------------------------------------------------
 //
 // *! ADDITIONAL COMMENTS :
@@ -48,22 +48,188 @@
 //-----------------------------------------------------------------------------------
 
 #include <fapi2.H>
-
+#include <p9_perv_scom_addresses.H>
+#include <p9_perv_scom_addresses_fld.H>
+#include <p9_misc_scom_addresses.H>
+#include <p9_quad_scom_addresses.H>
 //-----------------------------------------------------------------------------------
 // Structure definitions
 //-----------------------------------------------------------------------------------
 
 //function pointer typedef definition for HWP call support
 typedef fapi2::ReturnCode
-(*p9_tod_utils_get_tfmr_reg_FP_t) (const fapi2::Target<fapi2::TARGET_TYPE_CORE>&,
-                                   ecmdDataBufferBase& );
+(*p9_tod_utils_get_tfmr_reg_FP_t) (const fapi2::Target<fapi2::TARGET_TYPE_CORE>&, const uint8_t,
+                                   fapi2::buffer<uint64_t>& );
 
 typedef fapi2::ReturnCode
-(*p9_tod_utils_set_tfmr_reg_FP_t) (const fapi2::Target<fapi2::TARGET_TYPE_CORE>*&,
-                                   ecmdDataBufferBase& );
+(*p9_tod_utils_set_tfmr_reg_FP_t) (const fapi2::Target<fapi2::TARGET_TYPE_CORE>*&, const uint8_t,
+                                   fapi2::buffer<uint64_t>& );
 //-----------------------------------------------------------------------------------
 // Constant definitions
 //-----------------------------------------------------------------------------------
+// TOD-grid-cycles added to MDMT delay to account for slave staging
+const uint32_t MDMT_TOD_GRID_CYCLE_STAGING_DELAY = 6;
+
+// Duration of TOD GRID cycle in picoseconds
+const uint32_t  TOD_GRID_PS = 400;
+
+// Minimum/Maximum allowable delay for any node
+const uint32_t  MIN_TOD_DELAY = 0;
+const uint32_t  MAX_TOD_DELAY = 0xFF;
+
+// TOD operation delay times for HW/sim
+const uint32_t P9_TOD_UTILS_HW_NS_DELAY     = 50000;
+const uint32_t P9_TOD_UTILS_SIM_CYCLE_DELAY = 50000;
+
+// TOD retry count for hardware-cleared bits
+const uint32_t P9_TOD_UTIL_TIMEOUT_COUNT   = 20;
+
+//Bit definitions for TOD_M_PATH_CTRL_REG 1.1.1
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_512         = 0;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_128         = 1;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_64          = 2;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_32          = 3;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_4096        = 4;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_2048        = 5;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_1024        = 6;
+const uint32_t TOD_M_PATH_CTRL_REG_M_PATH_SYNC_CREATE_SPS_256         = 7;
+
+//Bit definitions for TOD_[PRI, SEC]_PORT_[0,1]_CTRL_REG 1.9.2, 1.9.3, 1.9.4, and 1.9.5
+const uint32_t TOD_PORT_CTRL_REG_RX_SEL           = 0;
+const uint32_t TOD_PORT_CTRL_REG_TX_X0_SEL        = 4;
+const uint32_t TOD_PORT_CTRL_REG_TX_X1_SEL        = 6;
+const uint32_t TOD_PORT_CTRL_REG_TX_X2_SEL        = 8;
+const uint32_t TOD_PORT_CTRL_REG_TX_X3_SEL        = 10;
+const uint32_t TOD_PORT_CTRL_REG_TX_X4_SEL        = 12;
+const uint32_t TOD_PORT_CTRL_REG_TX_X5_SEL        = 14;
+const uint32_t TOD_PORT_CTRL_REG_TX_X6_SEL        = 16;
+const uint32_t TOD_PORT_CTRL_REG_TX_X7_SEL        = 18;
+const uint32_t TOD_PORT_CTRL_REG_TX_X0_EN         = 20;
+const uint32_t TOD_PORT_CTRL_REG_TX_X1_EN         = 21;
+const uint32_t TOD_PORT_CTRL_REG_TX_X2_EN         = 22;
+const uint32_t TOD_PORT_CTRL_REG_TX_X3_EN         = 23;
+const uint32_t TOD_PORT_CTRL_REG_TX_X4_EN         = 24;
+const uint32_t TOD_PORT_CTRL_REG_TX_X5_EN         = 25;
+const uint32_t TOD_PORT_CTRL_REG_TX_X6_EN         = 26;
+const uint32_t TOD_PORT_CTRL_REG_TX_X7_EN         = 27;
+const uint32_t TOD_PORT_CTRL_REG_I_PATH_DELAY     = 32;
+
+const uint32_t TOD_PORT_CTRL_REG_RX_LEN           = 3;
+const uint32_t TOD_PORT_CTRL_REG_TX_LEN           = 2;
+const uint32_t TOD_PORT_CTRL_REG_I_PATH_DELAY_LEN = 8;
+
+const uint32_t TOD_PORT_CTRL_REG_RX_X0_SEL = 0x0;
+const uint32_t TOD_PORT_CTRL_REG_RX_X1_SEL = 0x1;
+const uint32_t TOD_PORT_CTRL_REG_RX_X2_SEL = 0x2;
+const uint32_t TOD_PORT_CTRL_REG_RX_X3_SEL = 0x3;
+const uint32_t TOD_PORT_CTRL_REG_RX_X4_SEL = 0x4;
+const uint32_t TOD_PORT_CTRL_REG_RX_X5_SEL = 0x5;
+const uint32_t TOD_PORT_CTRL_REG_RX_X6_SEL = 0x6;
+const uint32_t TOD_PORT_CTRL_REG_RX_X7_SEL = 0x7;
+
+
+const uint32_t TOD_PORT_CTRL_REG_S_PATH_0 = 0x0;
+const uint32_t TOD_PORT_CTRL_REG_S_PATH_1 = 0x1;
+const uint32_t TOD_PORT_CTRL_REG_M_PATH_0 = 0x2;
+const uint32_t TOD_PORT_CTRL_REG_M_PATH_1 = 0x3;
+
+//Bit definitions for TOD_S_PATH_CTRL_REG 1.9.6
+const uint32_t STEP_CHECK_CPS_DEVIATION_FACTOR_1 = 0;
+const uint32_t STEP_CHECK_CPS_DEVIATION_FACTOR_2 = 1;
+const uint32_t STEP_CHECK_CPS_DEVIATION_FACTOR_4 = 2;
+const uint32_t STEP_CHECK_CPS_DEVIATION_FACTOR_8 = 3;
+
+const uint32_t STEP_CHECK_CPS_DEVIATION_00_00_PCENT = 0x0;
+const uint32_t STEP_CHECK_CPS_DEVIATION_06_25_PCENT = 0x1;
+const uint32_t STEP_CHECK_CPS_DEVIATION_12_50_PCENT = 0x2;
+const uint32_t STEP_CHECK_CPS_DEVIATION_18_75_PCENT = 0x3;
+const uint32_t STEP_CHECK_CPS_DEVIATION_25_00_PCENT = 0x4;
+const uint32_t STEP_CHECK_CPS_DEVIATION_31_25_PCENT = 0x5;
+const uint32_t STEP_CHECK_CPS_DEVIATION_37_50_PCENT = 0x6;
+const uint32_t STEP_CHECK_CPS_DEVIATION_43_75_PCENT = 0x7;
+const uint32_t STEP_CHECK_CPS_DEVIATION_50_00_PCENT = 0x8;
+const uint32_t STEP_CHECK_CPS_DEVIATION_56_25_PCENT = 0x9;
+const uint32_t STEP_CHECK_CPS_DEVIATION_62_50_PCENT = 0xA;
+const uint32_t STEP_CHECK_CPS_DEVIATION_68_75_PCENT = 0xB;
+const uint32_t STEP_CHECK_CPS_DEVIATION_75_00_PCENT = 0xC;
+const uint32_t STEP_CHECK_CPS_DEVIATION_81_25_PCENT = 0xD;
+const uint32_t STEP_CHECK_CPS_DEVIATION_87_50_PCENT = 0xE;
+const uint32_t STEP_CHECK_CPS_DEVIATION_93_75_PCENT = 0xF;
+
+const uint32_t STEP_CHECK_VALIDITY_COUNT_1   = 0;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_2   = 1;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_4   = 2;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_8   = 3;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_16  = 4;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_32  = 5;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_64  = 6;
+const uint32_t STEP_CHECK_VALIDITY_COUNT_128 = 7;
+
+// Bit definitions for TOD_CHIP_CTRL_REG 1.9.17
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_8   = 0;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_4   = 1;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_2   = 2;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_1   = 3;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_128 = 4;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_64  = 5;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_32  = 6;
+const uint32_t TOD_CHIP_CTRL_REG_I_PATH_CORE_SYNC_PERIOD_SEL_16  = 7;
+
+const uint32_t TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VAL_1   = 0x1;
+const uint32_t TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VAL_2   = 0x2;
+const uint32_t TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VAL_3   = 0x3;
+const uint32_t TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VAL_4   = 0x4;
+
+//TODO fill these in with correct values
+const uint32_t SPRC_REG_SEL_TFMR_T0 = 8;
+const uint32_t SPRC_REG_SEL_TFMR_T1 = 9;
+const uint32_t SPRC_REG_SEL_TFMR_T2 = 10;
+const uint32_t SPRC_REG_SEL_TFMR_T3 = 11;
+const uint32_t SPRC_REG_SEL_TFMR_T4 = 12;
+const uint32_t SPRC_REG_SEL_TFMR_T5 = 13;
+const uint32_t SPRC_REG_SEL_TFMR_T6 = 14;
+const uint32_t SPRC_REG_SEL_TFMR_T7 = 15;
+const uint32_t SPRC_REG_SEL = 54;
+const uint32_t SPRC_REG_SEL_LEN = 7;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T0_SEL = 20;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T1_SEL = 21;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T2_SEL = 22;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T3_SEL = 23;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T4_SEL = 24;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T5_SEL = 25;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T6_SEL = 26;
+const uint32_t SPR_MODE_REG_MODE_SPRC_T7_SEL = 27;
+const uint32_t SPR_MODE_REG_MODE_SPRC0_SEL = 16;
+const uint32_t SPR_MODE_REG_MODE_SPRC_WR_EN = 3;
+
+const uint32_t TOD_TX_TTYPE_CTRL_REG_TX_TTYPE_PIB_MST_ADDR_CFG_C5 = 0x15;
+const uint32_t TOD_TX_TTYPE_CTRL_REG_TX_TTYPE_PIB_MST_ADDR_CFG = 24;
+const uint32_t TOD_TX_TTYPE_CTRL_REG_TX_TTYPE_PIB_MST_ADDR_CFG_LEN = 8;
+
+const uint32_t TFMR_TB_ECLIPZ = 14;
+const uint32_t TFMR_SYNC_BIT_SEL_LEN = 3;
+const uint32_t TFMR_SYNC_BIT_SEL = 11;
+const uint32_t TFMR_SYNC_BIT_SEL_16US = 4;
+const uint32_t TFMR_N_CLKS_PER_STEP_LEN = 2;
+const uint32_t TFMR_N_CLKS_PER_STEP = 8;
+const uint32_t TFMR_N_CLKS_PER_STEP_2CLK = 1;
+const uint32_t TFMR_N_CLKS_PER_STEP_4CLK  = 3;
+const uint32_t TFMR_MAX_CYC_BET_STEPS_LEN = 8;
+const uint32_t TFMR_MAX_CYC_BET_STEPS = 0;
+
+//TFMR register state bits and states
+const uint32_t TFMR_STATE_START_BIT = 28;
+const uint32_t TFMR_STATE_NUM_BITS = 4;
+const uint32_t TFMR_STATE_TB_RESET = 0;
+const uint32_t TFMR_STATE_TB_NOT_SET = 2;
+const uint32_t TFMR_STATE_TB_SYNC_WAIT = 6;
+const uint32_t TFMR_STATE_GET_TOD = 7;
+const uint32_t TFMR_STATE_TB_RUNNING = 8;
+const uint32_t TFMR_LOAD_TOD_MOD_TB = 16;
+const uint32_t TFMR_MOVE_CHIP_TOD_TO_TB = 18;
+
+//I don't see this one listed in the P9 documentation
+const uint32_t TOD_CHIP_CTRL_REG_LOW_ORDER_STEP_VAL_3F  = 0x3F;
 
 extern "C" {
 
@@ -83,9 +249,10 @@ extern "C" {
         XBUS1,
         XBUS2,
         XBUS3,
-        ABUS0,
-        ABUS1,
-        ABUS2
+        XBUS4,
+        XBUS5,
+        XBUS6,
+        XBUS7
     };
 
     enum tod_action
@@ -94,7 +261,8 @@ extern "C" {
         TOD_INIT,
         TOD_STATUS,
         TOD_CLEANUP,
-        TOD_CHECK_OSC
+        TOD_CHECK_OSC,
+        TOD_MOVE_TOD_TO_TB
     };
 
     // Input which determines the master oscillator to use
@@ -110,15 +278,15 @@ extern "C" {
 
     struct p9_tod_setup_conf_regs
     {
-        ecmdDataBufferBase tod_m_path_ctrl_reg;
-        ecmdDataBufferBase tod_pri_port_0_ctrl_reg;
-        ecmdDataBufferBase tod_pri_port_1_ctrl_reg;
-        ecmdDataBufferBase tod_sec_port_0_ctrl_reg;
-        ecmdDataBufferBase tod_sec_port_1_ctrl_reg;
-        ecmdDataBufferBase tod_s_path_ctrl_reg;
-        ecmdDataBufferBase tod_i_path_ctrl_reg;
-        ecmdDataBufferBase tod_pss_mss_ctrl_reg;
-        ecmdDataBufferBase tod_chip_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_m_path_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_pri_port_0_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_pri_port_1_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_sec_port_0_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_sec_port_1_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_s_path_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_i_path_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_pss_mss_ctrl_reg;
+        fapi2::buffer<uint64_t> tod_chip_ctrl_reg;
     };
 
     struct tod_topology_node
@@ -139,19 +307,23 @@ extern "C" {
 
 /// @brief Gets the value in the TFMR register
 /// @param[in] i_target       => P9 chip target
+/// @param[in] i_thread_num   => The thread number we want to run on
 /// @param[out] o_tfmr_val    => Value that is in the TFMR register
 /// @return FAPI_RC_SUCCESS if TFMR read is successful else FAPI or ECMD error is sent through
     fapi2::ReturnCode p9_tod_utils_get_tfmr_reg(
         const fapi2::Target<fapi2::TARGET_TYPE_CORE>& i_target,
-        ecmdDataBufferBase& o_tfmr_val);
+        const uint8_t i_thread_num,
+        fapi2::buffer<uint64_t>& o_tfmr_val);
 
 /// @brief Sets the value in the TFMR register
 /// @param[in] i_target => P9 chip target
+/// @param[in] i_thread_num => The thread number we want to run on
 /// @param[in] i_tfmr_val => Value that will be put in the TFMR register
 /// @return FAPI_RC_SUCCESS if TFMR write is successful else FAPI or ECMD error is sent through
     fapi2::ReturnCode p9_tod_utils_set_tfmr_reg(
         const fapi2::Target<fapi2::TARGET_TYPE_CORE>& i_target,
-        ecmdDataBufferBase& i_tfmr_val);
+        const uint8_t i_thread_num,
+        fapi2::buffer<uint64_t>& i_tfmr_val);
 
 } //extern "C"