diff --git a/src/core/sal/CMakeLists.txt b/src/core/sal/CMakeLists.txt
index 3e4ebc22..35360744 100644
--- a/src/core/sal/CMakeLists.txt
+++ b/src/core/sal/CMakeLists.txt
@@ -18,7 +18,7 @@ elseif(BUILD_GEM5)
 		SimulatorController.cc
 		perf/BreakpointBuffer.cc
 		gem5/Gem5Controller.cc
-		gem5/Gem5PCEvent.cc
+		gem5/Gem5PCEvents.cc
 	)
 elseif(BUILD_OVP)
 	set(SRCS
diff --git a/src/core/sal/gem5/FailGem5Device.cc b/src/core/sal/gem5/FailGem5Device.cc
index d10f6ebd..eac25f7f 100644
--- a/src/core/sal/gem5/FailGem5Device.cc
+++ b/src/core/sal/gem5/FailGem5Device.cc
@@ -1,16 +1,116 @@
-#include "FailGem5Device.hh"
+#include "FailGem5Device.hpp"
+
 #include "debug/Fail.hh"
+#include "sim/system.hh"
+#include "arch/arm/decoder.hh"
+#include "cpu/simple/base.hh"
 
 #include "../SALInst.hpp"
+#include "Gem5PCEvents.hpp"
+
+#include <bitset>
 
 FailGem5Device::FailGem5Device(Params *p)
 	: BasicPioDevice(p)
 {
 	pioSize = 0x60;
+}
 
+void FailGem5Device::startup()
+{
+	// TODO: Find a better way/position to start single stepping.
+	// It's not possible to fetch the instruction at Adress 0x0
+	m_BreakpointNotTaken = new Gem5InstructionEvent(this, &sys->pcEventQueue, 0x80000000);
 	fail::simulator.startup();
 }
 
+void FailGem5Device::activateSingleStepMode()
+{
+	setNextBreakpoints(sys->getThreadContext(0));
+}
+
+void FailGem5Device::deactivateSingleStepMode()
+{
+	clearBreakpoints();
+}
+
+void FailGem5Device::setNextBreakpoints(ThreadContext *tc)
+{
+	clearBreakpoints();
+
+	// Get the instruction at the current program counter
+	MachInst inst;
+	PCState pc = tc->pcState();
+	tc->getVirtProxy().readBlob(pc.pc(), (uint8_t*)&inst, sizeof(MachInst));
+
+	// Decode the instruction
+	ArmISA::Decoder* decoder = tc->getDecoderPtr();
+	BaseSimpleCPU* cpu = dynamic_cast<BaseSimpleCPU*>(tc->getCpuPtr());
+	Addr fetchPC = (pc.pc() & cpu->PCMask) + cpu->fetchOffset;
+	decoder->moreBytes(pc, fetchPC, inst);
+	StaticInstPtr si = decoder->decode(pc);
+
+	// Set breakpoints
+	// First breakpoint is always the next pc
+	// FIXME: Doesn't work properly in Thumb or Thumb2 mode
+	m_BreakpointNotTaken = new Gem5InstructionEvent(this, &sys->pcEventQueue, pc.npc());
+
+	// Second breakpoint will only be set if there is a possible change in control flow
+	DPRINTF(Fail, "Instruction Name: %s\n", si->getName());
+
+	if(si->getName().compare("bx") == 0)
+	{
+		DPRINTF(Fail, "BX instruction\n");
+		Addr target = tc->readIntReg(si->machInst.rm);
+		m_BreakpointTaken = new Gem5InstructionEvent(this, &sys->pcEventQueue, target);
+		
+		return;
+	}
+
+	// Check for a pop instruction with the pc as target
+	if(si->getName().compare("ldmstm") == 0)
+	{
+		std::bitset<16> regs(si->machInst.regList);
+		if(regs.test(15)) // Bit 15 = pc
+		{
+			DPRINTF(Fail, "LDMSTM with pc as target\n");
+			Addr mem = tc->readIntReg(si->machInst.rn) + 4*(regs.count()-1);
+			
+			Addr target;
+			tc->getVirtProxy().readBlob(mem, (uint8_t*)&target, sizeof(Addr));
+			DPRINTF(Fail, "Value loaded into pc: %x\n", target);
+			
+			m_BreakpointTaken = new Gem5InstructionEvent(this, &sys->pcEventQueue, target);
+		}
+		return;
+	}
+
+	PCState bpc;
+	if(si->hasBranchTarget(pc, tc, bpc))
+	{
+		DPRINTF(Fail, "Immediate branch\n");
+	    if (bpc.pc() != pc.npc())
+		{
+	        m_BreakpointTaken = new Gem5InstructionEvent(this, &sys->pcEventQueue, bpc.pc());
+		}
+	}
+}
+
+void FailGem5Device::clearBreakpoints()
+{
+	if(m_BreakpointTaken)
+	{
+		delete m_BreakpointTaken;
+		m_BreakpointTaken = 0;
+	}
+	if(m_BreakpointNotTaken)
+	{
+		delete m_BreakpointNotTaken;
+		m_BreakpointNotTaken = 0;
+	}
+}
+
+
 Tick FailGem5Device::read(PacketPtr pkt)
 {
 	return pioDelay;
diff --git a/src/core/sal/gem5/FailGem5Device.hh b/src/core/sal/gem5/FailGem5Device.hh
deleted file mode 100644
index 3601fcdd..00000000
--- a/src/core/sal/gem5/FailGem5Device.hh
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef __FAILGEM5_DEVICE_HH__
-#define __FAILGEM5_DEVICE_HH__
-
-#include "dev/io_device.hh"
-#include "params/FailGem5Device.hh"
-
-class FailGem5Device : public BasicPioDevice
-{
-	public:
-		typedef FailGem5DeviceParams Params;
-		
-		FailGem5Device(Params *p);
-
-		virtual Tick read(PacketPtr pkt);
-
-		virtual Tick write(PacketPtr pkt);
-};
-
-#endif // __FAILGEM5_DEVICE_HH__
diff --git a/src/core/sal/gem5/FailGem5Device.hpp b/src/core/sal/gem5/FailGem5Device.hpp
new file mode 100644
index 00000000..d097d8af
--- /dev/null
+++ b/src/core/sal/gem5/FailGem5Device.hpp
@@ -0,0 +1,32 @@
+#ifndef __FAILGEM5_DEVICE_HH__
+#define __FAILGEM5_DEVICE_HH__
+
+#include "dev/io_device.hh"
+#include "params/FailGem5Device.hh"
+#include "cpu/thread_context.hh"
+
+class Gem5InstructionEvent;
+
+class FailGem5Device : public BasicPioDevice
+{
+	
+public:
+	typedef FailGem5DeviceParams Params;
+	FailGem5Device(Params *p);
+	
+	void setNextBreakpoints(ThreadContext *tc);
+
+	virtual void startup();
+	virtual Tick read(PacketPtr pkt);
+	virtual Tick write(PacketPtr pkt);
+
+private:
+	Gem5InstructionEvent* m_BreakpointTaken;
+	Gem5InstructionEvent* m_BreakpointNotTaken;
+
+	void activateSingleStepMode();
+	void deactivateSingleStepMode();
+	void clearBreakpoints();
+};
+
+#endif // __FAILGEM5_DEVICE_HH__
diff --git a/src/core/sal/gem5/Gem5Listener.ah b/src/core/sal/gem5/Gem5Listener.ah
index caf39231..be3e556e 100644
--- a/src/core/sal/gem5/Gem5Listener.ah
+++ b/src/core/sal/gem5/Gem5Listener.ah
@@ -6,7 +6,7 @@
 
 #if defined(BUILD_GEM5) && defined(CONFIG_EVENT_BREAKPOINTS)
 
-#include "Gem5PCEvent.hh"
+#include "Gem5PCEvents.hpp"
 #include "sim/system.hh"
 
 aspect Gem5Listener
@@ -14,12 +14,12 @@ aspect Gem5Listener
 	advice "fail::BPSingleListener" : slice class
 	{
 	private:
-		Gem5PCEvent* m_Breakpoint;
+		Gem5BreakpointEvent* m_Breakpoint;
 	public:
 		virtual bool onAddition()
 		{
 			System* sys = *System::systemList.begin();
-			m_Breakpoint = new Gem5PCEvent(&sys->pcEventQueue, this->m_WatchInstrPtr);
+			m_Breakpoint = new Gem5BreakpointEvent(&sys->pcEventQueue, this->m_WatchInstrPtr);
 			
 			return true;
 		}
diff --git a/src/core/sal/gem5/Gem5PCEvent.cc b/src/core/sal/gem5/Gem5PCEvent.cc
deleted file mode 100644
index 84dc6ca3..00000000
--- a/src/core/sal/gem5/Gem5PCEvent.cc
+++ /dev/null
@@ -1,7 +0,0 @@
-#include "Gem5PCEvent.hh"
-#include "../SALInst.hpp"
-
-void Gem5PCEvent::process(ThreadContext *tc)
-{
-	fail::simulator.onBreakpoint(this->evpc, fail::ANY_ADDR);
-}
diff --git a/src/core/sal/gem5/Gem5PCEvent.hh b/src/core/sal/gem5/Gem5PCEvent.hh
deleted file mode 100644
index 54e89792..00000000
--- a/src/core/sal/gem5/Gem5PCEvent.hh
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef __GEM5_PCEVENT_HPP__
-  #define __GEM5_PCEVENT_HPP__
-
-#include "cpu/pc_event.hh"
-
-class Gem5PCEvent : public PCEvent
-{
-public:
-	Gem5PCEvent(PCEventQueue* queue, Addr ip)
-		: PCEvent(queue, "Fail Breakpoint", ip) {}
-	virtual void process(ThreadContext *tc);
-};
-
-#endif // __GEM5_PCEVENT_HPP__
diff --git a/src/core/sal/gem5/Gem5PCEvents.cc b/src/core/sal/gem5/Gem5PCEvents.cc
new file mode 100644
index 00000000..817d8f18
--- /dev/null
+++ b/src/core/sal/gem5/Gem5PCEvents.cc
@@ -0,0 +1,14 @@
+#include "Gem5PCEvents.hpp"
+#include "../SALInst.hpp"
+
+#include "cpu/thread_context.hh"
+
+void Gem5BreakpointEvent::process(ThreadContext *tc)
+{
+	fail::simulator.onBreakpoint(this->evpc, fail::ANY_ADDR);
+}
+
+void Gem5InstructionEvent::process(ThreadContext *tc)
+{
+	m_FailDevice->setNextBreakpoints(tc);
+}
diff --git a/src/core/sal/gem5/Gem5PCEvents.hpp b/src/core/sal/gem5/Gem5PCEvents.hpp
new file mode 100644
index 00000000..4230b432
--- /dev/null
+++ b/src/core/sal/gem5/Gem5PCEvents.hpp
@@ -0,0 +1,27 @@
+#ifndef __GEM5_PCEVENTS_HPP__
+  #define __GEM5_PCEVENTS_HPP__
+
+#include "cpu/pc_event.hh"
+
+#include "FailGem5Device.hpp"
+
+class Gem5BreakpointEvent : public PCEvent
+{
+public:
+	Gem5BreakpointEvent(PCEventQueue* queue, Addr ip)
+		: PCEvent(queue, "Fail breakpoint event", ip) {}
+	virtual void process(ThreadContext *tc);
+};
+
+class Gem5InstructionEvent : public PCEvent
+{
+public:
+	Gem5InstructionEvent(FailGem5Device* device, PCEventQueue* queue, Addr ip)
+		: PCEvent(queue, "Fail instruction event", ip), m_FailDevice(device) {}
+	virtual void process(ThreadContext *tc);
+
+private:
+	FailGem5Device* m_FailDevice;
+};
+
+#endif // __GEM5_PCEVENTS_HPP__