diff --git a/cpu.cpp b/cpu.cpp
index f097165..72a2bd5 100644
--- a/cpu.cpp
+++ b/cpu.cpp
@@ -1104,18 +1104,18 @@ void cpu::busError()
 {
 	fprintf(stderr, "BUS ERROR\n");
 
-	//  PSW = 177776
-	//  mov @#PSW, -(sp)
-	pushStack(getPSW());
+	trap(4);
+}
 
-	//  mov pc, -(sp)
+void cpu::trap(const uint16_t vector)
+{
+	pushStack(getPSW());
 	pushStack(getPC());
 
-	//  mov @#VEC+2, @#PSW
-	setPSW(b -> readWord(6));
+	setPSW(b->readWord(vector + 2));
+	setPC (b->readWord(vector + 0));
 
-	//  mov @#VEC, pc
-	setPC(b -> readWord(4));
+	fprintf(stderr, "TRAP %o: PC is now %06o\n", vector, getPC());
 }
 
 std::pair<std::string, int> cpu::addressing_to_string(const uint8_t mode_register, const uint16_t pc)
diff --git a/cpu.h b/cpu.h
index 4344a6f..843b50b 100644
--- a/cpu.h
+++ b/cpu.h
@@ -58,6 +58,7 @@ public:
 	uint16_t popStack();
 
 	void busError();
+	void trap(const uint16_t vector);
 
 	void setEmulateMFPT(const bool v) { emulateMFPT = v; }
 
diff --git a/rk05.cpp b/rk05.cpp
index 7c0f2a7..2c31fb1 100644
--- a/rk05.cpp
+++ b/rk05.cpp
@@ -4,6 +4,7 @@
 #include <string.h>
 
 #include "bus.h"
+#include "cpu.h"
 #include "error.h"
 #include "esp32.h"
 #include "gen.h"
@@ -133,6 +134,9 @@ void rk05::writeWord(const uint16_t addr, uint16_t v)
 	const int reg = (addr - RK05_BASE) / 2;
 	fprintf(stderr, "RK05 write %s/%o: %o\n", regnames[reg], addr, v);
 
+	D(fprintf(stderr, "set register %o to %o\n", addr, v);)
+	registers[reg] = v;
+
 	if (addr == RK05_CS) {
 		if (v & 1) { // GO
 			const int func = (v >> 1) & 7; // FUNCTION
@@ -206,9 +210,9 @@ void rk05::writeWord(const uint16_t addr, uint16_t v)
 				while(temp > 0) {
 					uint32_t cur = std::min(uint32_t(sizeof xfer_buffer), temp);
 
+#if defined(ESP32)
 					yield();
 
-#if defined(ESP32)
 					if (fh.read(xfer_buffer, cur) != size_t(cur))
 						D(fprintf(stderr, "RK05 fread error: %s\n", strerror(errno));)
 #else
@@ -249,17 +253,17 @@ void rk05::writeWord(const uint16_t addr, uint16_t v)
 				fprintf(stderr, "RK05 command %d UNHANDLED\n", func);
 			}
 
-			if (v & 64) { // bit 6, invoke interrupt when done vector address 220, see http://www.pdp-11.nl/peripherals/disk/rk05-info.html
-				fprintf(stderr, "RK05 HIER\n"); // FIXME
-			}
-
 			registers[(RK05_WC - RK05_BASE) / 2] = 0;
+
+			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);
+			}
 		}
 	}
 
-	D(fprintf(stderr, "set register %o to %o\n", addr, v);)
-	registers[reg] = v;
-
 #if defined(ESP32)
 	digitalWrite(LED_BUILTIN, HIGH);
 #endif