diff --git a/cpu.cpp b/cpu.cpp
index 6fccb6c..c5286be 100644
--- a/cpu.cpp
+++ b/cpu.cpp
@@ -13,6 +13,9 @@
 
 cpu::cpu(bus *const b, uint32_t *const event) : b(b), event(event)
 {
+	for(int level=0; level<8; level++)
+		queued_interrupts.insert({ level, { } });
+
 	reset();
 }
 
@@ -141,6 +144,48 @@ void cpu::setPSW_spl(const int v)
 	psw |= v << 5;
 }
 
+int cpu::getPSW_spl() const
+{
+	return (psw >> 5) & 7;
+}
+
+void cpu::setPSW(const uint16_t v)
+{
+	psw = v;
+}
+
+void cpu::check_queued_interrupts()
+{
+	uint8_t current_level = getPSW_spl();
+
+	uint8_t start_level = current_level <= 3 ? 0 : current_level + 1;
+
+	for(uint8_t i=start_level; i < 8; i++) {
+		auto interrupts = queued_interrupts.find(i);
+
+		if (interrupts->second.empty() == false) {
+			auto vector = interrupts->second.begin();
+
+			interrupts->second.erase(vector);
+
+			D(fprintf(stderr, "Invoking interrupt vector %o (IPL %d, current: %d)\n", *vector, i, current_level);)
+
+			trap(*vector, i);
+
+			break;
+		}
+	}
+}
+
+void cpu::queue_interrupt(const uint8_t level, const uint8_t vector)
+{
+	auto it = queued_interrupts.find(level);
+
+	it->second.insert(vector);
+
+	D(fprintf(stderr, "Queueing interrupt vector %o (IPL %d, current: %d), n: %zu\n", vector, level, getPSW_spl(), it->second.size());)
+}
+
 // GAM = general addressing modes
 uint16_t cpu::getGAM(const uint8_t mode, const uint8_t reg, const bool word_mode, const bool prev_mode)
 {
@@ -747,7 +792,7 @@ bool cpu::single_operand_instructions(const uint16_t instr)
 
 		case 0b000101111: { // TST/TSTB
 					  uint16_t v = getGAM(dst_mode, dst_reg, word_mode, false);
-					  setPSW_n(word_mode ? v & 128 : v & 32768);
+					  setPSW_n(SIGN(v, word_mode));
 					  setPSW_z(v == 0);
 					  setPSW_v(false);
 					  setPSW_c(false);
@@ -1161,18 +1206,20 @@ void cpu::busError()
 	trap(4);
 }
 
-void cpu::trap(const uint16_t vector)
+void cpu::trap(const uint16_t vector, const int new_ipl)
 {
 	uint16_t before_psw = getPSW();
 	uint16_t before_pc  = getPC();
 
 	// switch to kernel mode & update 'previous mode'
 	uint16_t new_psw = b->readWord(vector + 2) & 0147777;  // mask off old 'previous mode'
+	if (new_ipl != -1)
+		new_psw = (new_psw & ~0xe0) | (new_ipl << 5);
 	new_psw |= (before_psw >> 2) & 030000; // apply new 'previous mode'
 	setPSW(new_psw);
 
 	pushStack(before_psw);
-	pushStack(before_pc);
+	pushStack(before_pc);  // TODO: komt deze op de goede stack?
 
 	setPC(b->readWord(vector + 0));
 
@@ -1575,6 +1622,8 @@ void cpu::disassemble()
 
 void cpu::step()
 {
+	check_queued_interrupts();
+
 	uint16_t temp_pc = getPC();
 
 	if (temp_pc & 1)
diff --git a/cpu.h b/cpu.h
index 47075f6..c7c80c6 100644
--- a/cpu.h
+++ b/cpu.h
@@ -1,8 +1,10 @@
-// (C) 2018 by Folkert van Heusden
+// (C) 2018-2022 by Folkert van Heusden
 // Released under Apache License v2.0
 #pragma once
 
 #include <assert.h>
+#include <map>
+#include <set>
 #include <stdint.h>
 
 #include "bus.h"
@@ -10,20 +12,26 @@
 class cpu
 {
 private:
-	bool disas { false };
+	bool     disas { false };
 	uint16_t regs0_5[2][6]; // R0...5, selected by bit 11 in PSW, 
 	uint16_t sp[3 + 1]; // stackpointers, MF../MT.. select via 12/13 from PSW, others via 14/15
-	uint16_t pc { 0 };
-	uint16_t psw { 0 }, fpsr { 0 };
+	uint16_t pc    { 0 };
+	uint16_t psw   { 0 };
+	uint16_t fpsr  { 0 };
 	uint16_t stackLimitRegister { 0 };
 	bool resetFlag   { false };
 	bool runMode     { false };
 	bool emulateMFPT { false };
 
+	// level, vector
+	std::map<uint8_t, std::set<uint8_t> > queued_interrupts;
+
 	bus *const b { nullptr };
 
 	uint32_t *const event { nullptr };
 
+	void check_queued_interrupts();
+
 	uint16_t getRegister(const int nr, const bool MF_MT) const;
 	void setRegister(const int nr, const bool MF_MT, const uint16_t value);
 	void addRegister(const int nr, const bool MF_MT, const uint16_t value);
@@ -58,8 +66,10 @@ public:
 	void pushStack(const uint16_t v);
 	uint16_t popStack();
 
+	void queue_interrupt(const uint8_t level, const uint8_t vector);
+
 	void busError();
-	void trap(const uint16_t vector);
+	void trap(const uint16_t vector, const int new_ipl = -1);
 
 	void setEmulateMFPT(const bool v) { emulateMFPT = v; }
 
@@ -69,6 +79,7 @@ public:
 	bool getPSW_v() const;
 	bool getPSW_z() const;
 	bool getPSW_n() const;
+	int  getPSW_spl() const;
 	bool getBitPSW(const int bit) const;
 
 	void setPSW_c(const bool v);
@@ -79,7 +90,7 @@ public:
 	void setBitPSW(const int bit, const bool v);
 
 	uint16_t getPSW() const { return psw; }
-	void setPSW(const uint16_t v) { psw = v; }
+	void setPSW(const uint16_t v);
 
 	uint16_t getStackLimitRegister() { return stackLimitRegister; }
 	void setStackLimitRegister(const uint16_t v) { stackLimitRegister = v; }
diff --git a/rk05.cpp b/rk05.cpp
index 8d56828..54288a2 100644
--- a/rk05.cpp
+++ b/rk05.cpp
@@ -284,9 +284,9 @@ void rk05::writeWord(const uint16_t addr, uint16_t v)
 			registers[(RK05_DS - RK05_BASE) / 2] |= 64;  // drive ready
 			registers[(RK05_CS - RK05_BASE) / 2] |= 128;  // control ready
 
-			if (v & 64) { // bit 6, invoke interrupt when done vector address 220, see http://www.pdp-11.nl/peripherals/disk/rk05-info.html
-				b->getCpu()->trap(0220);
-			}
+			// bit 6, invoke interrupt when done vector address 220, see http://www.pdp-11.nl/peripherals/disk/rk05-info.html
+			if (v & 64)
+				b->getCpu()->queue_interrupt(5, 0220);
 		}
 	}