@@ -123,6 +123,7 @@ bool (*lookupIdentifier)(struct mDebuggerPlatform*, const char* name, int32_t* value, int* segment);
 
 	uint32_t (*getStackTraceMode)(struct mDebuggerPlatform*);
 	void (*setStackTraceMode)(struct mDebuggerPlatform*, uint32_t mode);
+	bool (*updateStackTrace)(struct mDebuggerPlatform* d);
 };
 
 struct mDebugger {

@@ -30,6 +30,7 @@ uint32_t frameBaseAddress;
 	void* regs;
 	bool finished;
 	bool breakWhenFinished;
+	bool interrupt;
 };
 
 DECLARE_VECTOR(mStackFrames, struct mStackFrame);

@@ -16,17 +16,42 @@ #include <stdint.h>
 
 DEFINE_VECTOR(ARMDebugBreakpointList, struct ARMDebugBreakpoint);
 
-static bool _updateStackTrace(struct mDebuggerPlatform* d, uint32_t pc) {
+static inline uint32_t ARMDebuggerGetInstructionLength(struct ARMCore* cpu) {
+	return cpu->cpsr.t == MODE_ARM ? WORD_SIZE_ARM : WORD_SIZE_THUMB;
+}
+
+static bool ARMDebuggerUpdateStackTraceInternal(struct mDebuggerPlatform* d, uint32_t pc) {
 	struct ARMDebugger* debugger = (struct ARMDebugger*) d;
 	struct ARMCore* cpu = debugger->cpu;
 	struct ARMInstructionInfo info;
 	uint32_t instruction = cpu->prefetch[0];
+	struct mStackTrace* stack = &d->p->stackTrace;
+	bool interrupt = false;
 	ARMDecodeARM(instruction, &info);
-	if (info.branchType == ARM_BRANCH_NONE) {
+
+	if (_ARMModeHasSPSR(cpu->cpsr.priv)) {
+		struct mStackFrame* irqFrame = mStackTraceGetFrame(stack, 0);
+		// TODO: uint32_t ivtBase = ARMControlRegIsVE(cpu->cp15.r1.c0) ? 0xFFFF0000 : 0x00000000;
+		uint32_t ivtBase = 0x00000000;
+		if (ivtBase <= pc && pc < ivtBase + 0x20 && !(irqFrame && _ARMModeHasSPSR(((struct ARMRegisterFile*)irqFrame->regs)->cpsr.priv))) {
+			// TODO: Potential enhancement opportunity: add break-on-exception mode
+			irqFrame = mStackTracePush(stack, pc, pc, cpu->gprs[ARM_SP], &cpu->regs);
+			irqFrame->interrupt = true;
+			interrupt = true;
+		}
+		char buffer[2048];
+		size_t len = sizeof(buffer) - 2;
+		d->trace(d, buffer, &len);
+	}
+
+	if (info.memory.format & ARM_MEMORY_WRITEBACK && info.memory.baseReg == ARM_SP) {
+		// Stack manipulation: see if it twiddles lr
+	}
+
+	if (info.branchType == ARM_BRANCH_NONE && !interrupt) {
 		return false;
 	}
 
-	struct mStackTrace* stack = &d->p->stackTrace;
 	struct mStackFrame* frame = mStackTraceGetFrame(stack, 0);
 	bool isCall = (info.branchType & ARM_BRANCH_LINKED);
 	uint32_t destAddress;
@@ -36,21 +61,23 @@ mStackTracePop(stack);
 		frame = NULL;
 	}
 
-	if (info.operandFormat & ARM_OPERAND_MEMORY_1) {
-		if (!isCall) {
-			return false;
-		}
-		// count number of set bits, gcc/clang will convert to intrinsics
+	if (interrupt && info.branchType == ARM_BRANCH_NONE) {
+		// The stack frame was already pushed up above, so there's no
+		// action necessary here, but we still want to check for a
+		// breakpoint down below.
+		//
+		// The first instruction could possibly be a call, which would
+		// need ANOTHER stack frame, so only skip if it's not.
+	} else if (info.operandFormat & ARM_OPERAND_MEMORY_1) {
+		// This is most likely ldmia ..., {..., pc}, which is a function return.
+		// To find which stack slot holds the return address, count number of set bits.
+		// (gcc/clang will convert the loop to intrinsics if available)
 		int regCount = 0;
-		uint32_t reglist = info.op1.immediate & 0xf;
-		printf("%d\n", reglist);
-		fflush(NULL);
+		uint32_t reglist = info.op1.immediate;
 		while (reglist != 0) {
 			reglist &= reglist - 1;
 			regCount++;
 		}
-		printf("->%d\n", regCount);
-		fflush(NULL);
 		uint32_t baseAddress = cpu->gprs[info.memory.baseReg] + ((regCount - 1) << 2);
 		destAddress = cpu->memory.load32(cpu, baseAddress, NULL);
 	} else if (info.operandFormat & ARM_OPERAND_IMMEDIATE_1) {
@@ -59,7 +86,7 @@ return false;
 		}
 		destAddress = info.op1.immediate + cpu->gprs[ARM_PC];
 	} else if (info.operandFormat & ARM_OPERAND_REGISTER_1) {
-		if (!isCall && info.op1.reg != ARM_LR) {
+		if (!isCall && info.op1.reg != ARM_LR && !(_ARMModeHasSPSR(cpu->cpsr.priv) && info.op1.reg == ARM_PC)) {
 			return false;
 		}
 		destAddress = cpu->gprs[info.op1.reg];
@@ -72,7 +99,8 @@ destAddress = cpu->memory.load32(cpu, destAddress, NULL);
 	}
 
 	if (isCall) {
-		frame = mStackTracePush(stack, pc, destAddress, cpu->gprs[ARM_SP], &cpu->regs);
+		int instructionLength = ARMDebuggerGetInstructionLength(debugger->cpu);
+		frame = mStackTracePush(stack, pc, destAddress + instructionLength, cpu->gprs[ARM_SP], &cpu->regs);
 		if (!(debugger->stackTraceMode & STACK_TRACE_BREAK_ON_CALL)) {
 			return false;
 		}
@@ -122,15 +150,9 @@ }
 
 static void ARMDebuggerCheckBreakpoints(struct mDebuggerPlatform* d) {
 	struct ARMDebugger* debugger = (struct ARMDebugger*) d;
-	int instructionLength;
-	enum ExecutionMode mode = debugger->cpu->cpsr.t;
-	if (mode == MODE_ARM) {
-		instructionLength = WORD_SIZE_ARM;
-	} else {
-		instructionLength = WORD_SIZE_THUMB;
-	}
+	int instructionLength = ARMDebuggerGetInstructionLength(debugger->cpu);
 	uint32_t pc = debugger->cpu->gprs[ARM_PC] - instructionLength;
-	if (debugger->stackTraceMode != STACK_TRACE_DISABLED && _updateStackTrace(d, pc)) {
+	if (debugger->stackTraceMode != STACK_TRACE_DISABLED && ARMDebuggerUpdateStackTraceInternal(d, pc)) {
 		return;
 	}
 	struct ARMDebugBreakpoint* breakpoint = _lookupBreakpoint(&debugger->breakpoints, pc);
@@ -165,12 +187,13 @@ static void ARMDebuggerListWatchpoints(struct mDebuggerPlatform*, struct mWatchpointList*);
 static void ARMDebuggerCheckBreakpoints(struct mDebuggerPlatform*);
 static bool ARMDebuggerHasBreakpoints(struct mDebuggerPlatform*);
 static void ARMDebuggerTrace(struct mDebuggerPlatform*, char* out, size_t* length);
-static void ARMDebuggerFormatRegisters(struct ARMRegisterFile* regs, char* delimiter, char* disassembly, char* out, size_t* length);
+static void ARMDebuggerFormatRegisters(struct ARMRegisterFile* regs, char* out, size_t* length);
 static void ARMDebuggerFrameFormatRegisters(struct mStackFrame* frame, char* out, size_t* length);
 static bool ARMDebuggerGetRegister(struct mDebuggerPlatform*, const char* name, int32_t* value);
 static bool ARMDebuggerSetRegister(struct mDebuggerPlatform*, const char* name, int32_t value);
 static uint32_t ARMDebuggerGetStackTraceMode(struct mDebuggerPlatform*);
 static void ARMDebuggerSetStackTraceMode(struct mDebuggerPlatform*, uint32_t);
+static bool ARMDebuggerUpdateStackTrace(struct mDebuggerPlatform* d);
 
 struct mDebuggerPlatform* ARMDebuggerPlatformCreate(void) {
 	struct mDebuggerPlatform* platform = (struct mDebuggerPlatform*) malloc(sizeof(struct ARMDebugger));
@@ -189,6 +212,7 @@ platform->getRegister = ARMDebuggerGetRegister;
 	platform->setRegister = ARMDebuggerSetRegister;
 	platform->getStackTraceMode = ARMDebuggerGetStackTraceMode;
 	platform->setStackTraceMode = ARMDebuggerSetStackTraceMode;
+	platform->updateStackTrace = ARMDebuggerUpdateStackTrace;
 	return platform;
 }
 
@@ -418,20 +442,22 @@ ARMDisassemble(&info, cpu->gprs[ARM_PC], disassembly + strlen("00000000: "), sizeof(disassembly) - strlen("00000000: "));
 		}
 	}
 
-	ARMDebuggerFormatRegisters(&cpu->regs, " | ", disassembly, out, length);
+	size_t regStringLen = *length;
+	ARMDebuggerFormatRegisters(&cpu->regs, out, &regStringLen);
+	*length = regStringLen + snprintf(out + regStringLen, *length - regStringLen, " | %s", disassembly);
 }
 
-static void ARMDebuggerFormatRegisters(struct ARMRegisterFile* regs, char* delimiter, char* disassembly, char* out, size_t* length) {
-	*length = snprintf(out, *length, "%08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X cpsr: %08X%s%s",
+static void ARMDebuggerFormatRegisters(struct ARMRegisterFile* regs, char* out, size_t* length) {
+	*length = snprintf(out, *length, "%08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X %08X cpsr: %08X",
 		               regs->gprs[0],  regs->gprs[1],  regs->gprs[2],  regs->gprs[3],
 		               regs->gprs[4],  regs->gprs[5],  regs->gprs[6],  regs->gprs[7],
 		               regs->gprs[8],  regs->gprs[9],  regs->gprs[10], regs->gprs[11],
 		               regs->gprs[12], regs->gprs[13], regs->gprs[14], regs->gprs[15],
-		               regs->cpsr.packed, delimiter, disassembly);
+		               regs->cpsr.packed);
 }
 
 static void ARMDebuggerFrameFormatRegisters(struct mStackFrame* frame, char* out, size_t* length) {
-	ARMDebuggerFormatRegisters((struct ARMRegisterFile*)&frame->regs, "", "", out, length);
+	ARMDebuggerFormatRegisters((struct ARMRegisterFile*)frame->regs, out, length);
 }
 
 bool ARMDebuggerGetRegister(struct mDebuggerPlatform* d, const char* name, int32_t* value) {
@@ -522,5 +548,15 @@ if (mode == STACK_TRACE_DISABLED && debugger->stackTraceMode != STACK_TRACE_DISABLED) {
 		mStackTraceClear(stack);
 	}
 	debugger->stackTraceMode = mode;
-	printf("SetStackTraceMode %d\n", mode);
+}
+
+static bool ARMDebuggerUpdateStackTrace(struct mDebuggerPlatform* d) {
+	struct ARMDebugger* debugger = (struct ARMDebugger*) d;
+	int instructionLength = ARMDebuggerGetInstructionLength(debugger->cpu);
+	uint32_t pc = debugger->cpu->gprs[ARM_PC] - instructionLength;
+	if (debugger->stackTraceMode != STACK_TRACE_DISABLED) {
+		return ARMDebuggerUpdateStackTraceInternal(d, pc);
+	} else {
+		return false;
+	}
 }

@@ -142,6 +142,18 @@ UNUSED(sig);
 	printf("No debugger attached!\n");
 }
 
+static bool CLIDebuggerCheckTraceMode(struct CLIDebugger* debugger, bool requireEnabled) {
+	struct mDebuggerPlatform* platform = debugger->d.platform;
+	if (!platform->getStackTraceMode) {
+		debugger->backend->printf(debugger->backend, "Stack tracing is not supported by this platform.\n");
+		return false;
+	} else if (requireEnabled && platform->getStackTraceMode(platform) == STACK_TRACE_DISABLED) {
+		debugger->backend->printf(debugger->backend, "Stack tracing is not enabled.\n");
+		return false;
+	}
+	return true;
+}
+
 static void _breakInto(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
 	UNUSED(debugger);
 	UNUSED(dv);
@@ -166,8 +178,12 @@ debugger->d.state = debugger->traceRemaining != 0 ? DEBUGGER_CALLBACK : DEBUGGER_RUNNING;
 }
 
 static void _next(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
+	struct mDebuggerPlatform* platform = debugger->d.platform;
 	UNUSED(dv);
 	debugger->d.core->step(debugger->d.core);
+	if (platform->getStackTraceMode && platform->getStackTraceMode(platform) != STACK_TRACE_DISABLED) {
+		platform->updateStackTrace(platform);
+	}
 	_printStatus(debugger, 0);
 }
 
@@ -334,6 +350,7 @@ }
 
 static void _reset(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
 	UNUSED(dv);
+	mStackTraceClear(&debugger->d.stackTrace);
 	debugger->d.core->reset(debugger->d.core);
 	_printStatus(debugger, 0);
 }
@@ -1010,6 +1027,18 @@ } else {
 			cliDebugger->backend->printf(cliDebugger->backend, "Hit illegal opcode\n");
 		}
 		break;
+	case DEBUGGER_ENTER_STACK:
+		if (info) {
+			if (info->type.st.traceType == STACK_TRACE_BREAK_ON_CALL) {
+				cliDebugger->backend->printf(cliDebugger->backend, "Hit function call at at 0x%08X\n", info->address);
+			} else {
+				cliDebugger->backend->printf(cliDebugger->backend, "Hit function return at at 0x%08X\n", info->address);
+			}
+		} else {
+			cliDebugger->backend->printf(cliDebugger->backend, "Hit function call or return\n");
+		}
+		_backtrace(cliDebugger, NULL);
+		break;
 	}
 }
 
@@ -1140,14 +1169,10 @@ return true;
 }
 
 static void _backtrace(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
-	struct mDebuggerPlatform* platform = debugger->d.platform;
-	if (!platform->getStackTraceMode) {
-		debugger->backend->printf(debugger->backend, "Stack tracing is not supported by this platform.\n");
-		return;
-	} else if (platform->getStackTraceMode(platform) == STACK_TRACE_DISABLED) {
-		debugger->backend->printf(debugger->backend, "Stack tracing is not enabled.\n");
+	if (!CLIDebuggerCheckTraceMode(debugger, true)) {
 		return;
 	}
+	struct mDebuggerPlatform* platform = debugger->d.platform;
 	struct mStackTrace* stack = &debugger->d.stackTrace;
 	size_t frames = mStackTraceGetDepth(stack);
 	if (dv && dv->type == CLIDV_INT_TYPE && dv->intValue < frames) {
@@ -1163,14 +1188,25 @@ }
 }
 
 static void _finish(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
+	UNUSED(dv);
+	if (!CLIDebuggerCheckTraceMode(debugger, true)) {
+		return;
+	}
+	struct mStackTrace* stack = &debugger->d.stackTrace;
+	struct mStackFrame* frame = mStackTraceGetFrame(stack, 0);
+	if (!frame) {
+		debugger->backend->printf(debugger->backend, "No current stack frame.\n");
+		return;
+	}
+	frame->breakWhenFinished = true;
+	_continue(debugger, dv);
 }
 
 static void _setStackTraceMode(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
-	printf("setStackTraceMode\n");
-	struct mDebuggerPlatform* platform = debugger->d.platform;
-	if (!platform->setStackTraceMode) {
-		debugger->backend->printf(debugger->backend, "Stack tracing is not supported by this platform.\n");
+	if (!CLIDebuggerCheckTraceMode(debugger, false)) {
 		return;
 	}
+	struct mDebuggerPlatform* platform = debugger->d.platform;
+	// TODO: get mode from vector
 	platform->setStackTraceMode(platform, STACK_TRACE_ENABLED);
 }

@@ -46,6 +46,7 @@ frame->frameBaseAddress = sp;
 	frame->regs = malloc(stack->registersSize);
 	frame->finished = false;
 	frame->breakWhenFinished = false;
+	frame->interrupt = false;
 	memcpy(frame->regs, regs, stack->registersSize);
 	return frame;
 }
@@ -74,11 +75,10 @@ return;
 	} else if (stack->formatRegisters) {
 		written += snprintf(out + written, *length - written, "(");
 		CHECK_LENGTH();
-		size_t formattedSize = *length - written;
-		stack->formatRegisters(stackFrame, out + written, &formattedSize);
-		written += formattedSize;
-		CHECK_LENGTH();
-		written += snprintf(out + written, *length - written, ")\n    ");
+		char buffer[1024];
+		size_t formattedSize = sizeof(buffer) - 2;
+		stack->formatRegisters(stackFrame, buffer, &formattedSize);
+		written += snprintf(out + written, *length - written, "%s)\n    ", buffer);
 		CHECK_LENGTH();
 	}
 	if (prevFrame) {
@@ -92,10 +92,9 @@ }
 
 void mStackTracePop(struct mStackTrace* stack) {
 	size_t depth = mStackTraceGetDepth(stack);
-	if (depth < 1) {
-		return;
+	if (depth > 0) {
+		struct mStackFrame* frame = mStackFramesGetPointer(&stack->stack, depth - 1);
+		free(frame->regs);
+		mStackFramesResize(&stack->stack, -1);
 	}
-	struct mStackFrame* frame = mStackFramesGetPointer(&stack->stack, depth - 1);
-	free(frame->regs);
-	mStackFramesResize(&stack->stack, -1);
 }

@@ -92,6 +92,7 @@ platform->d.getRegister = SM83DebuggerGetRegister;
 	platform->d.setRegister = SM83DebuggerSetRegister;
 	platform->d.getStackTraceMode = NULL;
 	platform->d.setStackTraceMode = NULL;
+	platform->d.updateStackTrace = NULL;
 	platform->printStatus = NULL;
 	return &platform->d;
 }