diff --git a/cpu.cpp b/cpu.cpp
index db28786..0d690dc 100644
--- a/cpu.cpp
+++ b/cpu.cpp
@@ -969,7 +969,7 @@ bool cpu::condition_code_operations(const uint16_t instr)
 void cpu::pushStack(const uint16_t v)
 {
 	if (getRegister(6) == stackLimitRegister) {
-		printf("stackLimitRegister reached\n");
+		printf("stackLimitRegister reached\n");  // TODO
 		exit(1);
 	}
 
@@ -984,13 +984,6 @@ uint16_t cpu::popStack()
 	return temp;
 }
 
-void cpu::switchModeToKernel()
-{
-	int previous_mode = (psw >> 14) & 3;
-	psw &= 0007777;
-	psw |= previous_mode << 12;
-}
-
 bool cpu::misc_operations(const uint16_t instr)
 {
 	switch(instr) {
@@ -1039,7 +1032,6 @@ bool cpu::misc_operations(const uint16_t instr)
 
 	if ((instr >> 8) == 0b10001001) { // TRAP
 		trap(034);
-		switchModeToKernel();
 		return true;
 	}
 
@@ -1089,13 +1081,20 @@ void cpu::busError()
 
 void cpu::trap(const uint16_t vector)
 {
-	pushStack(getPSW());
-	pushStack(getPC());
+	uint16_t before_psw = getPSW();
+	uint16_t before_pc  = getPC();
 
-	setPSW(b->readWord(vector + 2));
-	setPC (b->readWord(vector + 0));
+	// switch to kernel mode & update 'previous mode'
+	uint16_t new_psw = b->readWord(vector + 2) & 0147777;  // mask off old 'previous mode'
+	new_psw |= (before_psw >> 2) & 030000; // apply new 'previous mode'
+	setPSW(new_psw);
 
-	D(fprintf(stderr, "TRAP %o: PC is now %06o\n", vector, getPC());)
+	pushStack(before_psw);
+	pushStack(before_pc);
+
+	setPC(b->readWord(vector + 0));
+
+	D(fprintf(stderr, "TRAP %o: PC is now %06o, PSW is now %06o\n", vector, getPC(), new_psw);)
 }
 
 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 843b50b..abbad00 100644
--- a/cpu.h
+++ b/cpu.h
@@ -30,8 +30,6 @@ private:
 	uint16_t getGAM(const uint8_t mode, const uint8_t reg, const bool word_mode, const bool MF_MT);
 	void putGAM(const uint8_t mode, const int reg, const bool word_mode, const uint16_t value, const bool MF_FT);
 
-	void switchModeToKernel();
-
 	bool double_operand_instructions(const uint16_t instr);
 	bool additional_double_operand_instructions(const uint16_t instr);
 	bool single_operand_instructions(const uint16_t instr);