Doorbell Interrupt Base Support for Core/Thread Wakeup

This change includes:
    - Implementation of a generic KernelWorkItem Class
    - Kernel functionality for doorbell send to specific PIRs
    - Kernel changes to send core/thread Wakeup doorbells using
      doorbell_send() + placing KernelWorkItems on a cpu stack obj
      to be executed during doorbell wakeup
    - Kernel Interrupt Message handler changes to send wakeup msgs
    - Interrupt Resource Provider (INTRRP) Changes to handle
      wakeup msgs and monitor for timeouts
    - Changes to the IPL flow to invoke proper Core/Thread Wakeup
    - A basic outline (commented out) for how IPC messages can be
      implemented in the future

Change-Id: I547fb8719bac657def561565ae11ab18cde72096
CMVC-Prereq: 992722
RTC:137564
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/22815
Tested-by: Jenkins Server
Tested-by: FSP CI Jenkins
Reviewed-by: Andrew J. Geissler <andrewg@us.ibm.com>
Reviewed-by: Prachi Gupta <pragupta@us.ibm.com>
Reviewed-by: Daniel M. Crowell <dcrowell@us.ibm.com>

19 files changed, 385 insertions, 147 deletions

diff --git a/src/include/kernel/cpu.H b/src/include/kernel/cpu.H
index 963063b34..b967c1319 100644
--- a/src/include/kernel/cpu.H
+++ b/src/include/kernel/cpu.H
@@ -5,7 +5,9 @@
 /*                                                                        */
 /* OpenPOWER HostBoot Project                                             */
 /*                                                                        */
-/* COPYRIGHT International Business Machines Corp. 2010,2014              */
+/* Contributors Listed Below - COPYRIGHT 2010,2016                        */
+/* [+] International Business Machines Corp.                              */
+/*                                                                        */
 /*                                                                        */
 /* Licensed under the Apache License, Version 2.0 (the "License");        */
 /* you may not use this file except in compliance with the License.       */
@@ -29,9 +31,11 @@
 #define __KERNEL_CPU_H
 
 #include <kernel/types.h>
+#include <kernel/workitem.H>
 #include <arch/ppc.H>
 #include <builtins.h>
 #include <sys/sync.h>
+#include <util/lockfree/stack.H>
 
 // See BookIV PIR definition.
 #define KERNEL_MAX_SUPPORTED_NODES 8
@@ -97,6 +101,8 @@ struct cpu_t
     /** Sequence ID of CPU initialization. */
     uint64_t cpu_start_seqid;
 
+    /** Stack of WorkItems to be executed during doorbell wakeup */
+    Util::Lockfree::Stack<KernelWorkItem> doorbell_actions;
 };
 
 /** @fn getCpuId
diff --git a/src/include/kernel/doorbell.H b/src/include/kernel/doorbell.H
index d0c65be2c..8275c4fc7 100644
--- a/src/include/kernel/doorbell.H
+++ b/src/include/kernel/doorbell.H
@@ -38,4 +38,27 @@ void doorbell_broadcast();
  */
 void doorbell_clear();
 
+/** Send a doorbell exception to the given pir
+ *
+ *  @param i_pir - PIR to send doorbell exception to
+ */
+void doorbell_send(uint64_t i_pir);
+
+/** Initiate a thread/core wakeup using a doorbell
+ *
+ *  @param i_pir - PIR to send doorbell to wakeup
+ */
+void send_doorbell_wakeup(uint64_t i_pir);
+
+/* TODO RTC 150861
+ * send_doorbell_ipc
+ *
+ * void send_doorbell_ipc(cpu_t *i_cpu, uint64_t pir);
+**/
+
+enum
+{
+    _DOORBELL_MSG_TYPE = 0x0000000028000000, /// Comes from the ISA.
+};
+
 #endif
diff --git a/src/include/kernel/intmsghandler.H b/src/include/kernel/intmsghandler.H
index 8fd4e34b6..8f7c08559 100644
--- a/src/include/kernel/intmsghandler.H
+++ b/src/include/kernel/intmsghandler.H
@@ -145,14 +145,15 @@ class InterruptMsgHdlr : public MessageHandler
         static void addCpuCore(uint64_t i_pir);
 
         /**
-         * Issue an IPI to the core.
+         * Send message to interrupt resource provider (intrrp) in userspace to
+         * indicate a wakeup occurred for core/thread indicated by given pir.
+         * The intrrp monitors the expected cores/threads wakeup and will issue
+         * a timeout/error in the event that not all expected cores/threads
+         * send this message
          *
-         * @param[in] i_pir - The PIR of the CPU to send IPI at.
-         * @param[in] i_favor - How favored the interrupt is; 0 = most favored
-         *                      254 = least favored, 255 = no interrupt
-         *                      Default 0x1 - IPI from kernel side (wakeup)
+         * @param[in] i_pir - The PIR of the CPU to send doorbell to.
          */
-        static void sendIPI(uint64_t i_pir, uint8_t i_favor = 0x1);
+        static void sendThreadWakeupMsg(uint64_t i_pir);
 
         /**
          * Issue the sbe/mailbox workaround (issue a mbox EOI to mailbox)
diff --git a/src/include/kernel/workitem.H b/src/include/kernel/workitem.H
new file mode 100644
index 000000000..3a41cc526
--- /dev/null
+++ b/src/include/kernel/workitem.H
@@ -0,0 +1,68 @@
+/* IBM_PROLOG_BEGIN_TAG                                                   */
+/* This is an automatically generated prolog.                             */
+/*                                                                        */
+/* $Source: src/include/kernel/workitem.H $                               */
+/*                                                                        */
+/* OpenPOWER HostBoot Project                                             */
+/*                                                                        */
+/* Contributors Listed Below - COPYRIGHT 2016                             */
+/* [+] International Business Machines Corp.                              */
+/*                                                                        */
+/*                                                                        */
+/* Licensed under the Apache License, Version 2.0 (the "License");        */
+/* you may not use this file except in compliance with the License.       */
+/* You may obtain a copy of the License at                                */
+/*                                                                        */
+/*     http://www.apache.org/licenses/LICENSE-2.0                         */
+/*                                                                        */
+/* Unless required by applicable law or agreed to in writing, software    */
+/* distributed under the License is distributed on an "AS IS" BASIS,      */
+/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
+/* implied. See the License for the specific language governing           */
+/* permissions and limitations under the License.                         */
+/*                                                                        */
+/* IBM_PROLOG_END_TAG                                                     */
+
+#ifndef __KERNEL_WORKITEM_H
+#define __KERNEL_WORKITEM_H
+
+//The WorkItem class is a generic base class for a work item.
+class KernelWorkItem
+{
+    public:
+        //Virtual operator to be defined by child classes to define proper
+        // behavior in that situation
+        virtual void operator() (void);
+
+        /** Default destructor */
+        virtual ~KernelWorkItem() = default;
+
+        //Next Pointer is needed to be used with the lockfree::Stack container
+        KernelWorkItem* next;
+};
+
+//A work item to be created/executed during a CPU core/thread
+// wakeup scenario
+class CpuWakeupDoorbellWorkItem : public KernelWorkItem
+{
+    public:
+        //Implement operator() function
+        void operator() (void);
+
+        //No data to clean up, use default destructor
+        ~CpuWakeupDoorbellWorkItem() = default;
+};
+
+
+/* TODO RTC 150861
+
+//A work item to be created/executed during an IPC scenario
+class IPCDoorbellWorkItem : public KernelWorkItem
+{
+    public:
+        //Implement operator() function
+        void operator() (void);
+};
+**/
+#endif
+
diff --git a/src/include/sys/msg.h b/src/include/sys/msg.h
index e55f20665..b161a91e8 100644
--- a/src/include/sys/msg.h
+++ b/src/include/sys/msg.h
@@ -5,7 +5,9 @@
 /*                                                                        */
 /* OpenPOWER HostBoot Project                                             */
 /*                                                                        */
-/* COPYRIGHT International Business Machines Corp. 2010,2014              */
+/* Contributors Listed Below - COPYRIGHT 2010,2016                        */
+/* [+] International Business Machines Corp.                              */
+/*                                                                        */
 /*                                                                        */
 /* Licensed under the Apache License, Version 2.0 (the "License");        */
 /* you may not use this file except in compliance with the License.       */
@@ -61,6 +63,8 @@ enum msg_sys_types_t
 
     MSG_INTR_EXTERN,    //!< Msg sent from kernel to user space on ext intr
     MSG_INTR_ADD_CPU,   //!< Add cpu core, data[0] = cpuid (PIR)
+    MSG_INTR_CPU_WAKEUP, //!< Msg sent from kernel to user space on cpu wakeup
+                         // data[0] = cpuid (PIR)
     MSG_INTR_ISSUE_SBE_MBOX_WA,   //!< Issue EOI to mailbox
 
 };
@@ -119,6 +123,7 @@ enum msg_root_queue_types_t
  */
 
 
+
 // Message queue interfaces.
 
 /** @fn msg_q_create
diff --git a/src/include/usr/isteps/istep16list.H b/src/include/usr/isteps/istep16list.H
index 9a928dea1..ad1bba7d4 100644
--- a/src/include/usr/isteps/istep16list.H
+++ b/src/include/usr/isteps/istep16list.H
@@ -5,7 +5,7 @@
 /*                                                                        */
 /* OpenPOWER HostBoot Project                                             */
 /*                                                                        */
-/* Contributors Listed Below - COPYRIGHT 2012,2015                        */
+/* Contributors Listed Below - COPYRIGHT 2012,2016                        */
 /* [+] International Business Machines Corp.                              */
 /*                                                                        */
 /*                                                                        */
@@ -118,11 +118,8 @@ namespace   INITSERVICE
         },
         {
                 ISTEPNAME(16,02,"host_activate_slave_cores"),
-                // @todo RTC:137389 - removed for P9 bringup
-                //ISTEP_16::call_host_activate_slave_cores,
-                //{ START_FN, EXT_IMAGE, NORMAL_IPL_OP | MPIPL_OP, true }
-                NULL,
-                { NONE, EXT_IMAGE, IPL_NOOP, false }
+                ISTEP_16::call_host_activate_slave_cores,
+                { START_FN, EXT_IMAGE, NORMAL_IPL_OP | MPIPL_OP, true }
         },
         {
                 ISTEPNAME(16,03,"host_secure_rng"),
diff --git a/src/kernel/cpumgr.C b/src/kernel/cpumgr.C
index 1aef65650..c1733bb7c 100644
--- a/src/kernel/cpumgr.C
+++ b/src/kernel/cpumgr.C
@@ -5,7 +5,7 @@
 /*                                                                        */
 /* OpenPOWER HostBoot Project                                             */
 /*                                                                        */
-/* Contributors Listed Below - COPYRIGHT 2010,2015                        */
+/* Contributors Listed Below - COPYRIGHT 2010,2016                        */
 /* [+] International Business Machines Corp.                              */
 /*                                                                        */
 /*                                                                        */
@@ -45,6 +45,7 @@
 #include <kernel/hbterminatetypes.H>
 #include <kernel/kernel_reasoncodes.H>
 #include <kernel/cpuid.H>
+#include <kernel/doorbell.H>
 
 cpu_t** CpuManager::cv_cpus[KERNEL_MAX_SUPPORTED_NODES];
 bool CpuManager::cv_shutdown_requested = false;
@@ -193,7 +194,7 @@ void CpuManager::startCPU(ssize_t i)
     // Initialize CPU structure.
     if (NULL == cv_cpus[nodeId][cpuId])
     {
-        printk("Starting CPU %ld...", i);
+        printk("Starting CPU with pir %ld...", i);
         cpu_t* cpu = cv_cpus[nodeId][cpuId] = new cpu_t();
 
         // Initialize CPU.
@@ -434,8 +435,21 @@ void CpuManager::startCore(uint64_t pir,uint64_t i_threads)
     }
     __sync_synchronize();
 
+    //Send a message to userspace that a core with this base pir is being added
+    // userspace will know which threads on the core to expect already
     InterruptMsgHdlr::addCpuCore(pir);
 
+    for(size_t i = 0; i < threads; i++)
+    {
+        // Only wakeup the threads we were told to wakeup
+        if( i_threads & (0x8000000000000000 >> i) )
+        {
+            printk("Dbell wkup pir %ld\n", pir + i);
+            //Initiate the Doorbell for this core/pir
+            send_doorbell_wakeup(pir + i);
+        }
+    }
+
     return;
 };
 
diff --git a/src/kernel/doorbell.C b/src/kernel/doorbell.C
index 33b782bf7..4b99e7aee 100644
--- a/src/kernel/doorbell.C
+++ b/src/kernel/doorbell.C
@@ -24,11 +24,16 @@
 /* IBM_PROLOG_END_TAG                                                     */
 #include <stdint.h>
 #include <arch/ppc.H>
+#include <kernel/task.H>
+#include <kernel/taskmgr.H>
+#include <kernel/cpu.H>
+#include <kernel/doorbell.H>
+#include <kernel/console.H>
 #include <kernel/cpumgr.H>
 
 void doorbell_clear()
 {
-    register uint64_t msgtype = 0x0000000028000000; /// Comes from the ISA.
+    register uint64_t msgtype = _DOORBELL_MSG_TYPE;
     asm volatile("msgclr %0" :: "r" (msgtype));
 
     return;
@@ -36,9 +41,10 @@ void doorbell_clear()
 
 void doorbell_broadcast()
 {
+    /*TODO RTC 152189
     size_t threadCount = CpuManager::getThreadCount();
 
-    uint64_t msgtype = 0x0000000028000000;  /// Comes from the ISA.
+    uint64_t msgtype = _DOORBELL_MSG_TYPE;
     uint64_t thread = getPIR() & (threadCount - 1);
     for (size_t i = 0; i < threadCount; ++i)
     {
@@ -48,4 +54,42 @@ void doorbell_broadcast()
             asm volatile("msgsnd %0" :: "r" (msg));
         }
     }
+    **/
 }
+
+void doorbell_send(uint64_t i_pir)
+{
+    uint64_t msgtype = _DOORBELL_MSG_TYPE;
+    register uint64_t msg = msgtype | i_pir;
+    asm volatile("msgsnd %0" :: "r" (msg));
+
+    return;
+}
+
+void send_doorbell_wakeup(uint64_t i_pir)
+{
+    cpu_t *l_cpu = CpuManager::getCpu(i_pir);
+
+    printkd("send_doorbell_wakeup to pir: %lx\n", i_pir);
+    //Create WorkItem and put on the stack to be executed during doorbell
+    // execution
+    KernelWorkItem* l_work = new CpuWakeupDoorbellWorkItem();
+    l_cpu->doorbell_actions.push(l_work);
+    //Send doorbell to wakeup core/thread
+    doorbell_send(i_pir);
+}
+
+/*
+TODO RTC 150861
+void send_doorbell_ipc(uint64_t i_pir)
+{
+    cpu_t *l_cpu = getCpu(i_pir)
+
+    printk("send_doorbell_ipc to pir: %lx\n", i_pir);
+    //Create WorkItem and put on the stack to be executed during doorbell
+    // execution (if needed, otherwise can likely delete and just send doorbell)
+    KernelWorkItem* l_work = new IpcDoorbellWorkItem();
+    l_cpu->doorbell_actions.push(l_work);
+    doorbell_send(i_pir)
+}
+**/
diff --git a/src/kernel/intmsghandler.C b/src/kernel/intmsghandler.C
index 10c1dcfd8..301be0f05 100644
--- a/src/kernel/intmsghandler.C
+++ b/src/kernel/intmsghandler.C
@@ -113,7 +113,7 @@ void InterruptMsgHdlr::handleInterrupt()
     }
     else
     {
-        printk("InterrurptMsgHdlr got called before IPC was setup\n");
+        printk("InterruptMsgHdlr got called before IPC was setup\n");
         // The INTR mmio base address is not yet available via the attributes.
         // If we get here during an MPIPL then the BAR value could be read
         // from the ICP BAR SCOM register, however, since this value will
@@ -165,28 +165,30 @@ void InterruptMsgHdlr::addCpuCore(uint64_t i_pir)
     {
         // To avoid conflict with interrupts on thread i_pir, change the key
         // for the message to be an invalid PIR.
-        uint64_t pir_key = i_pir | 0x8000000000000000ul;
+        uint64_t pir_key = i_pir | 0x4000000000000000ul;
 
         cv_instance->iv_lock.lock();
         cv_instance->sendMessage(MSG_INTR_ADD_CPU,
                                  (void*)pir_key,(void *)i_pir,t);
         cv_instance->iv_lock.unlock();
+        printkd("InterruptMsgHdlr::addCpuCore MSG_INTR_ADD_CPU message "
+               "sent for pir: %lx\n", i_pir);
     }
 }
 
-void InterruptMsgHdlr::sendIPI(uint64_t i_pir, uint8_t i_favor)
+void InterruptMsgHdlr::sendThreadWakeupMsg(uint64_t i_pir)
 {
-    uint64_t mfrrAddress = cv_ipc_base_address;
-    mfrrAddress += mmio_offset(i_pir);
-    mfrrAddress += MFRR_ADDR_OFFSET;
-
-    mfrrAddress |= 0x8000000000000000ul;
-
-    register uint8_t data = i_favor;
+    if(cv_instance)
+    {
+        // To avoid conflict with interrupts on thread i_pir, change the key
+        // for the message to be an invalid PIR.
+        uint64_t pir_key = i_pir | 0x8000000000000000ul;
 
-    eieio(); sync();
-    MAGIC_INSTRUCTION(MAGIC_SIMICS_CORESTATESAVE);
-    asm volatile("stbcix %0,0,%1" :: "r" (data) , "r" (mfrrAddress));
+        cv_instance->iv_lock.lock();
+        cv_instance->sendMessage(MSG_INTR_CPU_WAKEUP,
+                                 (void*)pir_key,(void *)i_pir,NULL);
+        cv_instance->iv_lock.unlock();
+    }
 }
 
 MessageHandler::HandleResult InterruptMsgHdlr::handleResponse
diff --git a/src/kernel/ipc.C b/src/kernel/ipc.C
index 9f0eaa908..e7a9ead9a 100644
--- a/src/kernel/ipc.C
+++ b/src/kernel/ipc.C
@@ -5,7 +5,7 @@
 /*                                                                        */
 /* OpenPOWER HostBoot Project                                             */
 /*                                                                        */
-/* Contributors Listed Below - COPYRIGHT 2013,2015                        */
+/* Contributors Listed Below - COPYRIGHT 2013,2016                        */
 /* [+] International Business Machines Corp.                              */
 /*                                                                        */
 /*                                                                        */
@@ -87,8 +87,11 @@ int KernelIpc::send(uint64_t i_q, msg_t * i_msg)
 
     printkd("IPC send from PIR %lx to PIR %x\n",getPIR(),p_dest->pir);
 
+    /* TODO RTC 150861
     // send IPI - use this_node + 10 as favor level of interrupt
-    InterruptMsgHdlr::sendIPI(p_dest->pir,this_node + 0x10);
+    //P8 Call: InterruptMsgHdlr::sendIPI(p_dest->pir,this_node + 0x10);
+    //P9 Call (likely): send_doorbell_ipc(p_dest->pir);
+    **/
 
     // The message allocation is freed here to make msg_send for IPC
     // messages behave the same as non-IPC msg_send; that is, the message
diff --git a/src/kernel/makefile b/src/kernel/makefile
index ea9666b44..cf5a69aa7 100644
--- a/src/kernel/makefile
+++ b/src/kernel/makefile
@@ -65,6 +65,7 @@ OBJS += ipc.o
 OBJS += machchk.o
 
 OBJS += doorbell.o
+OBJS += workitem.o
 
 include ${ROOTPATH}/config.mk
 
diff --git a/src/kernel/misc.C b/src/kernel/misc.C
index 080c0adf3..3b25ae705 100644
--- a/src/kernel/misc.C
+++ b/src/kernel/misc.C
@@ -5,7 +5,7 @@
 /*                                                                        */
 /* OpenPOWER HostBoot Project                                             */
 /*                                                                        */
-/* Contributors Listed Below - COPYRIGHT 2011,2015                        */
+/* Contributors Listed Below - COPYRIGHT 2011,2016                        */
 /* [+] International Business Machines Corp.                              */
 /*                                                                        */
 /*                                                                        */
@@ -41,7 +41,7 @@
 #include <kernel/ipc.H>
 #include <kernel/timemgr.H>
 #include <util/singleton.H>
-
+#include <kernel/doorbell.H>
 
 extern "C"
     void kernel_shutdown(size_t, uint64_t, uint64_t, uint64_t,
@@ -340,8 +340,10 @@ namespace KernelMisc
 
         //Issue sbe master workaround
         InterruptMsgHdlr::issueSbeMboxWA();
+
         // NOTE: The cpu_t structures for theads 1:3 were created
         //       during init (CpuManager::init).
+        //
 
         #ifdef HOSTBOOT_REAL_WINKLE
         // @todo- RTC 141924 Start the other threads 1:3 in a new manner
@@ -349,6 +351,7 @@ namespace KernelMisc
         //  Maybe something like:   sendIPI(..) or  addCpuCore(..)
         //  Need interrupt code in place for this.
 
+
         #else
         // get other 3 threads going in SIMICs for now
         MAGIC_INSTRUCTION(MAGIC_WAKE_OTHER_THREADS);
@@ -371,6 +374,7 @@ namespace KernelMisc
             // get new core going in SIMICS
             MAGIC_INSTRUCTION(MAGIC_WAKE_FUSED_THREADS);
             #endif
+
         } // end if fused core mode
 
     }
@@ -440,7 +444,7 @@ namespace KernelMisc
                 {
                     if (slave->winkled)
                     {
-                        InterruptMsgHdlr::sendIPI(i);
+                        InterruptMsgHdlr::sendThreadWakeupMsg(i);
                     }
                 }
             }
diff --git a/src/kernel/syscall.C b/src/kernel/syscall.C
index d7e729d80..6f087f021 100644
--- a/src/kernel/syscall.C
+++ b/src/kernel/syscall.C
@@ -55,6 +55,17 @@ extern "C"
 void kernel_execute_hype_doorbell()
 {
     task_t* t = TaskManager::getCurrentTask();
+    doorbell_clear();
+
+    //Execute all work items on doorbell_actions stack
+    KernelWorkItem *l_work = t->cpu->doorbell_actions.pop();
+    while(l_work != NULL)
+    {
+        //Execute Work Item and then delete it
+        (*l_work)();
+        delete l_work;
+        l_work = t->cpu->doorbell_actions.pop();
+    }
 
     if (t->cpu->idle_task == t)
     {
@@ -63,8 +74,6 @@ void kernel_execute_hype_doorbell()
     }
 
     DeferredQueue::execute();
-
-    doorbell_clear();
 }
 
 extern "C"
@@ -401,7 +410,7 @@ namespace Systemcalls
 
         if (m->type >= MSG_FIRST_SYS_TYPE)
         {
-            printkd("Invalid message type for msg_sendrecv, type=%d.\n",
+            printk("Invalid message type for msg_sendrecv, type=%d.\n",
                     m->type);
             TASK_SETRTN(t, -EINVAL);
             return;
diff --git a/src/kernel/workitem.C b/src/kernel/workitem.C
new file mode 100644
index 000000000..a883cdb8f
--- /dev/null
+++ b/src/kernel/workitem.C
@@ -0,0 +1,50 @@
+/* IBM_PROLOG_BEGIN_TAG                                                   */
+/* This is an automatically generated prolog.                             */
+/*                                                                        */
+/* $Source: src/kernel/workitem.C $                                       */
+/*                                                                        */
+/* OpenPOWER HostBoot Project                                             */
+/*                                                                        */
+/* Contributors Listed Below - COPYRIGHT 2016                             */
+/* [+] International Business Machines Corp.                              */
+/*                                                                        */
+/*                                                                        */
+/* Licensed under the Apache License, Version 2.0 (the "License");        */
+/* you may not use this file except in compliance with the License.       */
+/* You may obtain a copy of the License at                                */
+/*                                                                        */
+/*     http://www.apache.org/licenses/LICENSE-2.0                         */
+/*                                                                        */
+/* Unless required by applicable law or agreed to in writing, software    */
+/* distributed under the License is distributed on an "AS IS" BASIS,      */
+/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
+/* implied. See the License for the specific language governing           */
+/* permissions and limitations under the License.                         */
+/*                                                                        */
+/* IBM_PROLOG_END_TAG                                                     */
+
+#include <arch/ppc.H>
+#include <kernel/workitem.H>
+#include <kernel/console.H>
+#include <kernel/intmsghandler.H>
+
+//Define the desired behavior for a CPU core/thread
+// wakeup scenario
+void CpuWakeupDoorbellWorkItem::operator() (void)
+{
+    size_t pir = getPIR();
+    printk("Wkup pir %ld done\n", pir);
+    //Send message to the intrrp in userspace indicating this pir has woken up
+    // There is a task associated with the intrrp that monitors that the proper
+    // cores/threads have woken up
+    InterruptMsgHdlr::sendThreadWakeupMsg(pir);
+    return;
+}
+
+/*TODO RTC 150861
+void IpcDoorbellWorkItem::operator() (void)
+{
+    //Decide what needs to be done when the Ipc Doorbell is received
+    // (if anything)
+}
+**/
diff --git a/src/makefile b/src/makefile
index 6b512b9d1..38dd589f9 100644
--- a/src/makefile
+++ b/src/makefile
@@ -117,6 +117,7 @@ DIRECT_BOOT_OBJECTS += vfs_init.o
 DIRECT_BOOT_OBJECTS += cpuid.o
 DIRECT_BOOT_OBJECTS += stdlib.o
 DIRECT_BOOT_OBJECTS += assert.o
+DIRECT_BOOT_OBJECTS += workitem.o
 
 BASE_MODULES += trace
 BASE_MODULES += errl
diff --git a/src/usr/intr/intrrp.C b/src/usr/intr/intrrp.C
index e4abde595..91bca3c11 100644
--- a/src/usr/intr/intrrp.C
+++ b/src/usr/intr/intrrp.C
@@ -180,7 +180,10 @@ errlHndl_t IntrRp::_init()
         //  so unmask those interrupts
         l_err = unmaskInterruptSource(LSI_PSU);
 
-
+        //Set value for enabled threads
+        uint64_t l_en_threads = get_enabled_threads();
+        TRACDCOMP(g_trac_intr, "IntrRp::_init() Threads enabled:"
+                                " %lx", l_en_threads);
     } while(0);
 
     return l_err;
@@ -421,11 +424,7 @@ void IntrRp::msgHandler()
                     uint64_t l_data0 = (l_xirr_pir & 0xFFFFFFFF);
                     PIR_t pir = static_cast<PIR_t>(l_data0);
 
-                    uint64_t baseAddr = iv_baseAddr + cpuOffsetAddr(pir);
-                    uint32_t * xirrAddress =
-                        reinterpret_cast<uint32_t*>(baseAddr + XIRR_OFFSET);
-
-                    TRACFCOMP(g_trac_intr,
+                    TRACDCOMP(g_trac_intr,
                               "External Interrupt received. XIRR=%x, PIR=%x",
                               xirr,pir.word);
                     //An external interrupt comes from two paths
@@ -467,15 +466,6 @@ void IntrRp::msgHandler()
                                       (uint32_t) type, rc);
                         }
                     }
-                    else if (type == INTERPROC_XISR)
-                    {
-                        // Ignore "spurious" IPIs (handled below).
-
-                        // Note that we could get an INTERPROC interrupt
-                        // and handle it through the above registration list
-                        // as well.  This is to catch the case where no one
-                        // has registered for an IPI.
-                    }
                     else if (type == LSI_PSU)
                     {
                         TRACDCOMP(g_trac_intr, "PSU Interrupt Detected");
@@ -490,89 +480,78 @@ void IntrRp::msgHandler()
                                   "Ignoring it.",
                                   (uint32_t)type);
                     }
+                }
+                break;
 
-                    // Handle IPIs special since they're used for waking up
-                    // cores and have special clearing requirements.
-                    if (type == INTERPROC_XISR)
-                    {
-                        // Clear IPI request.
-                        volatile uint8_t * mfrr =
-                            reinterpret_cast<uint8_t*>(baseAddr + MFRR_OFFSET);
+            case MSG_INTR_CPU_WAKEUP:
+                {
+                    uint64_t l_xirr_pir = msg->data[0];
+                    uint64_t l_data0 = (l_xirr_pir & 0xFFFFFFFF);
+                    PIR_t l_pir = static_cast<PIR_t>(l_data0);
+                    PIR_t l_core_pir = l_pir;
+                    l_core_pir.threadId = 0;
 
-                        TRACFCOMP( g_trac_intr,"mfrr = %x",*mfrr);
+                    if (iv_ipisPending.count(l_core_pir))
+                    {
+                        TRACFCOMP(g_trac_intr,INFO_MRK
+                                  "IntrRp::msgHandler Doorbell wakeup received"
+                                  " for %d", l_pir.word);
 
-                        (*mfrr) = 0xff;
-                        eieio();  // Force mfrr clear before xirr EIO.
+                        IPI_Info_t& ipiInfo = iv_ipisPending[l_core_pir];
+                        ipiInfo.first &=
+                           ~(0x8000000000000000 >> l_pir.threadId);
 
-                        // Deal with pending IPIs.
-                        PIR_t core_pir = pir; core_pir.threadId = 0;
-                        if (iv_ipisPending.count(core_pir))
+                        if (0 == ipiInfo.first)
                         {
-                            TRACFCOMP(g_trac_intr,INFO_MRK
-                                      "IPI wakeup received for %d", pir.word);
-
-                            IPI_Info_t& ipiInfo = iv_ipisPending[core_pir];
-
-                            ipiInfo.first &=
-                                ~(0x8000000000000000 >> pir.threadId);
-
-                            if (0 == ipiInfo.first)
-                            {
-                                msg_t* ipiMsg = ipiInfo.second;
-                                iv_ipisPending.erase(core_pir);
-
-                                ipiMsg->data[1] = 0;
-                                msg_respond(iv_msgQ, ipiMsg);
-                            }
-                            else
-                            {
-                                TRACDCOMP(g_trac_intr,INFO_MRK
-                                          "IPI still pending for %x",
-                                          ipiInfo.first);
-                            }
+                            msg_t* ipiMsg = ipiInfo.second;
+                            iv_ipisPending.erase(l_core_pir);
 
+                            ipiMsg->data[1] = 0;
+                            msg_respond(iv_msgQ, ipiMsg);
                         }
-
-                        // Writing the XIRR with the same value read earlier
-                        // to signal an EOI.
-                        xirr |= CPPR_MASK;  //set all CPPR bits - allow any INTR
-                        *xirrAddress = xirr;
-
-                        TRACDCOMP(g_trac_intr,
-                                  "EOI issued. XIRR=%x, PIR=%x",
-                                  xirr,pir);
-
-                        // Now handle any IPC messages
-                        // If something is registered for IPIs
-                        // and msg is ready, then handle
-                        if(r != iv_registry.end() &&
-                           (KernelIpc::ipc_data_area.msg_queue_id !=
-                           IPC_DATA_AREA_CLEAR) &&
-                            (KernelIpc::ipc_data_area.msg_queue_id !=
-                             IPC_DATA_AREA_LOCKED))
+                        else
                         {
-                            msg_q_t msgQ = r->second.msgQ;
-
-                            msg_t * rmsg = msg_allocate();
-                            rmsg->type = r->second.msgType;
-                            rmsg->data[0] = type;  // interrupt type
-                            rmsg->data[1] = l_xirr_pir;
-                            rmsg->extra_data = NULL;
-
-                            int rc = msg_sendrecv_noblk(msgQ, rmsg, iv_msgQ);
-                            if(rc)
-                            {
-                                TRACFCOMP(g_trac_intr,ERR_MRK
-                                          "IPI Interrupt received, but could "
-                                          "not send message to the registered "
-                                          "handler. Ignoring it. rc = %d",
-                                          rc);
-                            }
+                            TRACFCOMP(g_trac_intr,INFO_MRK
+                                      "IPI still pending for %x",
+                                      ipiInfo.first);
                         }
+
                     }
                 }
                 break;
+/*TODO RTC 150861 -- I think a new IPC message type needs to be defined.
+ *                   And the code below should be executed when this new message
+ *                   type is received. The Kernel will send this message to
+ *                   here (this intrrp code) during doorbell wakeup.
+
+                    // Now handle any IPC messages
+                    // If something is registered for IPIs
+                    // and msg is ready, then handle
+                    if(r != iv_registry.end() &&
+                       (KernelIpc::ipc_data_area.msg_queue_id !=
+                       IPC_DATA_AREA_CLEAR) &&
+                        (KernelIpc::ipc_data_area.msg_queue_id !=
+                         IPC_DATA_AREA_LOCKED))
+                    {
+                        msg_q_t msgQ = r->second.msgQ;
 
+                        msg_t * rmsg = msg_allocate();
+                        rmsg->type = r->second.msgType;
+                        rmsg->data[0] = type;  // interrupt type
+                        rmsg->data[1] = l_xirr_pir;
+                        rmsg->extra_data = NULL;
+
+                        int rc = msg_sendrecv_noblk(msgQ, rmsg, iv_msgQ);
+                        if(rc)
+                        {
+                            TRACFCOMP(g_trac_intr,ERR_MRK
+                                      "IPI Interrupt received, but could "
+                                      "not send message to the registered "
+                                      "handler. Ignoring it. rc = %d",
+                                      rc);
+                        }
+                    }
+**/
             case MSG_INTR_EOI:
                 {
                     // Use standrard EOI (End of Interrupt) sequence
@@ -662,13 +641,16 @@ void IntrRp::msgHandler()
                 }
                 break;
 
-#ifdef CONFIG_ENABLE_P9_IPI
             //  Called when a new cpu becomes active other than the master
             //  Expect a call for each new core
             case MSG_INTR_ADD_CPU:
                 {
+                    //Get the base PIR sent from the kernel
                     PIR_t pir = msg->data[1];
+                    //No need to care about thread ID as that will be gathered
+                    //  below
                     pir.threadId = 0;
+                    //Push back base core PIR for later use
                     iv_cpuList.push_back(pir);
 
                     TRACFCOMP(g_trac_intr,"Add CPU group[%d], chip[%d],"
@@ -676,40 +658,31 @@ void IntrRp::msgHandler()
                               pir.groupId, pir.chipId, pir.coreId,
                               pir.threadId);
 
-                    size_t threads = cpu_thread_count();
+                    //Get threads to be enabled so they will be monitored
                     uint64_t en_threads = get_enabled_threads();
-
                     iv_ipisPending[pir] = IPI_Info_t(en_threads, msg);
 
-                    for(size_t thread = 0; thread < threads; ++thread)
-                    {
-                        // Skip threads that we shouldn't be starting
-                        if( !(en_threads & (0x8000000000000000>>thread)) )
-                        {
-                            TRACDCOMP(g_trac_intr,"MSG_INTR_ADD_CPU: Skipping thread %d",thread);
-                            continue;
-                        }
-                        pir.threadId = thread;
-                        //wh_p9 initInterruptPresenter(pir);
-                        sendIPI(pir);
-                    }
-
-                    pir.threadId = 0;
+                    //Create handleCpuTimeout task - this task will monitor
+                    // for wakeup messages from each individual expected
+                    // thread to be sent.
                     task_create(handleCpuTimeout,
                                 reinterpret_cast<void*>(pir.word));
+                    TRACFCOMP(g_trac_intr, "handleCpuTimeout task started"
+                              " responding to kernel message");
                 }
                 break;
-#endif
             case MSG_INTR_ADD_CPU_TIMEOUT:
                 {
                     PIR_t pir = msg->data[0];
+                    TRACDCOMP("IntrRp::msgHandler() CPU Timeout Message "
+                              "received for: %x", pir.word);
                     size_t count = msg->data[1];
 
                     if(iv_ipisPending.count(pir))
                     {
                         if (count < CPU_WAKEUP_INTERVAL_COUNT)
                         {
-                            TRACDCOMP(g_trac_intr,
+                            TRACFCOMP(g_trac_intr,
                                       INFO_MRK "Cpu wakeup pending on %x",
                                       pir.word);
 
@@ -2768,6 +2741,8 @@ void* INTR::IntrRp::handleCpuTimeout(void* _pir)
     msg->data[0] = pir;
     msg_q_t intr_msgQ = msg_q_resolve(VFS_ROOT_MSG_INTR);
 
+    TRACFCOMP( g_trac_intr,"handleCpuTimeout for pir: %lx", pir);
+
     do
     {
         // Sleep for the right amount.
@@ -2838,3 +2813,20 @@ void INTR::drainQueue()
     }
     //else no queue, no need to do anything
 }
+
+uint64_t INTR::get_enabled_threads( void )
+{
+    TARGETING::Target* sys = NULL;
+    TARGETING::targetService().getTopLevelTarget(sys);
+    assert( sys != NULL );
+    uint64_t en_threads = sys->getAttr<TARGETING::ATTR_ENABLED_THREADS>();
+    if( en_threads == 0 )
+    {
+        //TODO RTC 151022
+        //Read <SBE memory area> for enabled threads value
+        //  and set attribute appropriately
+        en_threads = 0xF000000000000000; //Enable all the threads
+        sys->setAttr<TARGETING::ATTR_ENABLED_THREADS>(en_threads);
+    }
+    return en_threads;
+}
diff --git a/src/usr/intr/intrrp.H b/src/usr/intr/intrrp.H
index a5ef9cddc..5141544b0 100644
--- a/src/usr/intr/intrrp.H
+++ b/src/usr/intr/intrrp.H
@@ -67,6 +67,12 @@ namespace INTR
         return r.u32;
     }
 
+    /**
+     * @brief Utility function to get the list of enabled threads
+     * @return Bitstring of enabled threads
+     */
+    uint64_t get_enabled_threads( void );
+
     class IntrRp
     {
         public:
diff --git a/src/usr/isteps/istep16/call_host_activate_master.C b/src/usr/isteps/istep16/call_host_activate_master.C
index a90a97549..213f04596 100644
--- a/src/usr/isteps/istep16/call_host_activate_master.C
+++ b/src/usr/isteps/istep16/call_host_activate_master.C
@@ -107,6 +107,9 @@ void* call_host_activate_master (void *io_pArgs)
             TRACFCOMP( ISTEPS_TRACE::g_trac_isteps_trace,
                        "startDeadManLoop SUCCESS"  );
         }
+        //Need to indicate to PHYP to save HRMOR and other SPR Data to be
+        // applied during wakeup
+        MAGIC_INSTRUCTION(MAGIC_SIMICS_CORESTATESAVE);
 
         //Because of a bug in how the SBE injects the IPI used to wake
         //up the master core, need to ensure no mailbox traffic
diff --git a/src/usr/isteps/istep16/call_host_activate_slave_cores.C b/src/usr/isteps/istep16/call_host_activate_slave_cores.C
index 6e4977524..1d521213e 100644
--- a/src/usr/isteps/istep16/call_host_activate_slave_cores.C
+++ b/src/usr/isteps/istep16/call_host_activate_slave_cores.C
@@ -65,16 +65,22 @@ void* call_host_activate_slave_cores (void *io_pArgs)
 
     // @@@@@    CUSTOM BLOCK:   @@@@@
 
-    uint64_t l_masterCoreID = task_getcpuid() & ~7;
+    uint64_t l_masterCoreID = PIR_t::coreFromPir(task_getcpuid());
 
     TargetHandleList l_cores;
     getAllChiplets(l_cores, TYPE_CORE);
+    uint32_t l_numCores = 0;
 
     for(TargetHandleList::const_iterator
         l_core = l_cores.begin();
         l_core != l_cores.end();
         ++l_core)
     {
+        TRACFCOMP( ISTEPS_TRACE::g_trac_isteps_trace,
+                   "Iterating all cores in system - "
+                   "This is core: %d", l_numCores);
+        l_numCores += 1;
+
         ConstTargetHandle_t l_processor = getParentChip(*l_core);
 
         CHIP_UNIT_ATTR l_coreId =
@@ -86,20 +92,23 @@ void* call_host_activate_slave_cores (void *io_pArgs)
         TARGETING::Target* sys = NULL;
         TARGETING::targetService().getTopLevelTarget(sys);
         assert( sys != NULL );
-        uint64_t en_threads = sys->getAttr<ATTR_ENABLED_THREADS>();
 
         const fapi2::Target<fapi2::TARGET_TYPE_CORE> l_fapi2_coreTarget(
               const_cast<TARGETING::Target*> (*l_core));
 
+        //Determine PIR and threads to enable for this core
         uint64_t pir = PIR_t(l_logicalGroupId, l_chipId, l_coreId).word;
+        uint64_t en_threads = sys->getAttr<ATTR_ENABLED_THREADS>();
+        TRACFCOMP( ISTEPS_TRACE::g_trac_isteps_trace,
+                   "pir for this core is: %lx", pir);
 
         if (pir != l_masterCoreID)
         {
             TRACFCOMP( ISTEPS_TRACE::g_trac_isteps_trace,
-                       "call_host_activate_slave_cores: Waking %x",
+                       "call_host_activate_slave_cores: Waking %x.",
                        pir );
 
-            int rc = cpu_start_core(pir,en_threads);
+            int rc = cpu_start_core(pir, en_threads);
 
             // Handle time out error
             if (-ETIME == rc)