@@ -48,6 +48,7 @@ - Debugger: Watchpoints now report address watched (fixes #68)
  - GBA: Add API for getting Configuration structs for overrides and input
  - GBA: Refactor gba-sensors and gba-gpio into gba-hardware
  - GBA: Refactor gba directory, dropping gba- prefix and making supervisor directory
+ - Debugger: Add support for soft breakpoints
 
 0.1.1: (2015-01-24)
 Bugfixes:

@@ -121,6 +121,8 @@ void (*processEvents)(struct ARMCore* cpu);
 	void (*swi16)(struct ARMCore* cpu, int immediate);
 	void (*swi32)(struct ARMCore* cpu, int immediate);
 	void (*hitIllegal)(struct ARMCore* cpu, uint32_t opcode);
+	void (*bkpt16)(struct ARMCore* cpu, int immediate);
+	void (*bkpt32)(struct ARMCore* cpu, int immediate);
 	void (*readCPSR)(struct ARMCore* cpu);
 
 	void (*hitStub)(struct ARMCore* cpu, uint32_t opcode);

@@ -625,7 +625,7 @@ DEFINE_INSTRUCTION_ARM(MRC, ARM_STUB)
 
 // Begin miscellaneous definitions
 
-DEFINE_INSTRUCTION_ARM(BKPT, ARM_STUB) // Not strictly in ARMv4T, but here for convenience
+DEFINE_INSTRUCTION_ARM(BKPT, cpu->irqh.bkpt32(cpu, ((opcode >> 4) & 0xFFF0) | (opcode & 0xF))); // Not strictly in ARMv4T, but here for convenience
 DEFINE_INSTRUCTION_ARM(ILL, ARM_ILL) // Illegal opcode
 
 DEFINE_INSTRUCTION_ARM(MSR,

@@ -381,7 +381,7 @@ THUMB_STORE_POST_BODY;
 	cpu->gprs[ARM_SP] = address)
 
 DEFINE_INSTRUCTION_THUMB(ILL, ARM_ILL)
-DEFINE_INSTRUCTION_THUMB(BKPT, ARM_STUB)
+DEFINE_INSTRUCTION_THUMB(BKPT, cpu->irqh.bkpt16(cpu, opcode & 0xFF);)
 DEFINE_INSTRUCTION_THUMB(B,
 	int16_t immediate = (opcode & 0x07FF) << 5;
 	cpu->gprs[ARM_PC] += (((int32_t) immediate) >> 4);

@@ -35,6 +35,8 @@ static void _reset(struct CLIDebugger*, struct CLIDebugVector*);
 static void _readHalfword(struct CLIDebugger*, struct CLIDebugVector*);
 static void _readWord(struct CLIDebugger*, struct CLIDebugVector*);
 static void _setBreakpoint(struct CLIDebugger*, struct CLIDebugVector*);
+static void _setBreakpointARM(struct CLIDebugger*, struct CLIDebugVector*);
+static void _setBreakpointThumb(struct CLIDebugger*, struct CLIDebugVector*);
 static void _clearBreakpoint(struct CLIDebugger*, struct CLIDebugVector*);
 static void _setWatchpoint(struct CLIDebugger*, struct CLIDebugVector*);
 static void _writeByte(struct CLIDebugger*, struct CLIDebugVector*);
@@ -47,6 +49,8 @@ static uint32_t _printLine(struct CLIDebugger* debugger, uint32_t address, enum ExecutionMode mode);
 
 static struct CLIDebuggerCommandSummary _debuggerCommands[] = {
 	{ "b", _setBreakpoint, CLIDVParse, "Set a breakpoint" },
+	{ "b/a", _setBreakpointARM, CLIDVParse, "Set a software breakpoint as ARM" },
+	{ "b/t", _setBreakpointThumb, CLIDVParse, "Set a software breakpoint as Thumb" },
 	{ "break", _setBreakpoint, CLIDVParse, "Set a breakpoint" },
 	{ "c", _continue, 0, "Continue execution" },
 	{ "continue", _continue, 0, "Continue execution" },
@@ -129,6 +133,12 @@ }
 
 static void _next(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
 	UNUSED(dv);
+	if (debugger->d.currentBreakpoint) {
+		if (debugger->d.currentBreakpoint->isSw && debugger->d.setSoftwareBreakpoint) {
+			debugger->d.setSoftwareBreakpoint(&debugger->d, debugger->d.currentBreakpoint->address, debugger->d.currentBreakpoint->sw.mode, &debugger->d.currentBreakpoint->sw.opcode);
+		}
+		debugger->d.currentBreakpoint = 0;
+	}
 	ARMRun(debugger->d.cpu);
 	_printStatus(debugger, 0);
 }
@@ -385,6 +395,24 @@ return;
 	}
 	uint32_t address = dv->intValue;
 	ARMDebuggerSetBreakpoint(&debugger->d, address);
+}
+
+static void _setBreakpointARM(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
+	if (!dv || dv->type != CLIDV_INT_TYPE) {
+		printf("%s\n", ERROR_MISSING_ARGS);
+		return;
+	}
+	uint32_t address = dv->intValue;
+	ARMDebuggerSetSoftwareBreakpoint(&debugger->d, address, MODE_ARM);
+}
+
+static void _setBreakpointThumb(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {
+	if (!dv || dv->type != CLIDV_INT_TYPE) {
+		printf("%s\n", ERROR_MISSING_ARGS);
+		return;
+	}
+	uint32_t address = dv->intValue;
+	ARMDebuggerSetSoftwareBreakpoint(&debugger->d, address, MODE_THUMB);
 }
 
 static void _clearBreakpoint(struct CLIDebugger* debugger, struct CLIDebugVector* dv) {

@@ -6,13 +6,22 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
 #include "debugger.h"
 
 #include "arm.h"
+#include "isa-inlines.h"
 
 #include "memory-debugger.h"
 
 const uint32_t ARM_DEBUGGER_ID = 0xDEADBEEF;
 
+static struct DebugBreakpoint* _lookupBreakpoint(struct DebugBreakpoint* breakpoints, uint32_t address) {
+	for (; breakpoints; breakpoints = breakpoints->next) {
+		if (breakpoints->address == address) {
+			return breakpoints;
+		}
+	}
+	return 0;
+}
+
 static void _checkBreakpoints(struct ARMDebugger* debugger) {
-	struct DebugBreakpoint* breakpoint;
 	int instructionLength;
 	enum ExecutionMode mode = debugger->cpu->cpsr.t;
 	if (mode == MODE_ARM) {
@@ -20,15 +29,14 @@ instructionLength = WORD_SIZE_ARM;
 	} else {
 		instructionLength = WORD_SIZE_THUMB;
 	}
-	for (breakpoint = debugger->breakpoints; breakpoint; breakpoint = breakpoint->next) {
-		if (breakpoint->address + instructionLength == (uint32_t) debugger->cpu->gprs[ARM_PC]) {
-			struct DebuggerEntryInfo info = {
-				.address = breakpoint->address
-			};
-			ARMDebuggerEnter(debugger, DEBUGGER_ENTER_BREAKPOINT, &info);
-			break;
-		}
+	struct DebugBreakpoint* breakpoint = _lookupBreakpoint(debugger->breakpoints, debugger->cpu->gprs[ARM_PC] - instructionLength);
+	if (!breakpoint) {
+		return;
 	}
+	struct DebuggerEntryInfo info = {
+		.address = breakpoint->address
+	};
+	ARMDebuggerEnter(debugger, DEBUGGER_ENTER_BREAKPOINT, &info);
 }
 
 static void ARMDebuggerInit(struct ARMCore*, struct ARMComponent*);
@@ -47,6 +55,7 @@ debugger->state = DEBUGGER_RUNNING;
 	debugger->breakpoints = 0;
 	debugger->originalMemory = cpu->memory;
 	debugger->watchpoints = 0;
+	debugger->currentBreakpoint = 0;
 	if (debugger->init) {
 		debugger->init(debugger);
 	}
@@ -78,6 +87,12 @@ debugger->paused(debugger);
 		} else {
 			debugger->state = DEBUGGER_RUNNING;
 		}
+		if (debugger->state != DEBUGGER_PAUSED && debugger->currentBreakpoint) {
+			if (debugger->currentBreakpoint->isSw && debugger->setSoftwareBreakpoint) {
+				debugger->setSoftwareBreakpoint(debugger, debugger->currentBreakpoint->address, debugger->currentBreakpoint->sw.mode, &debugger->currentBreakpoint->sw.opcode);
+			}
+			debugger->currentBreakpoint = 0;
+		}
 		break;
 	case DEBUGGER_SHUTDOWN:
 		return;
@@ -86,6 +101,27 @@ }
 
 void ARMDebuggerEnter(struct ARMDebugger* debugger, enum DebuggerEntryReason reason, struct DebuggerEntryInfo* info) {
 	debugger->state = DEBUGGER_PAUSED;
+	struct ARMCore* cpu = debugger->cpu;
+	cpu->nextEvent = 0;
+	if (reason == DEBUGGER_ENTER_BREAKPOINT) {
+		struct DebugBreakpoint* breakpoint = _lookupBreakpoint(debugger->swBreakpoints, _ARMPCAddress(cpu));
+		debugger->currentBreakpoint = breakpoint;
+		if (breakpoint && breakpoint->isSw) {
+			info->address = breakpoint->address;
+			if (debugger->clearSoftwareBreakpoint) {
+				debugger->clearSoftwareBreakpoint(debugger, breakpoint->address, breakpoint->sw.mode, breakpoint->sw.opcode);
+			}
+
+			// Roll back CPU state
+			if (breakpoint->sw.mode == MODE_ARM) {
+				cpu->gprs[ARM_PC] -= WORD_SIZE_ARM;
+			} else {
+				cpu->gprs[ARM_PC] -= WORD_SIZE_THUMB;
+			}
+			cpu->prefetch[1] = cpu->prefetch[0];
+			cpu->prefetch[0] = breakpoint->sw.opcode;
+		}
+	}
 	if (debugger->entered) {
 		debugger->entered(debugger, reason, info);
 	}
@@ -95,7 +131,25 @@ void ARMDebuggerSetBreakpoint(struct ARMDebugger* debugger, uint32_t address) {
 	struct DebugBreakpoint* breakpoint = malloc(sizeof(struct DebugBreakpoint));
 	breakpoint->address = address;
 	breakpoint->next = debugger->breakpoints;
+	breakpoint->isSw = false;
 	debugger->breakpoints = breakpoint;
+}
+
+bool ARMDebuggerSetSoftwareBreakpoint(struct ARMDebugger* debugger, uint32_t address, enum ExecutionMode mode) {
+	uint32_t opcode;
+	if (!debugger->setSoftwareBreakpoint || !debugger->setSoftwareBreakpoint(debugger, address, mode, &opcode)) {
+		return false;
+	}
+
+	struct DebugBreakpoint* breakpoint = malloc(sizeof(struct DebugBreakpoint));
+	breakpoint->address = address;
+	breakpoint->next = debugger->swBreakpoints;
+	breakpoint->isSw = true;
+	breakpoint->sw.opcode = opcode;
+	breakpoint->sw.mode = mode;
+	debugger->swBreakpoints = breakpoint;
+
+	return true;
 }
 
 void ARMDebuggerClearBreakpoint(struct ARMDebugger* debugger, uint32_t address) {

@@ -22,6 +22,11 @@
 struct DebugBreakpoint {
 	struct DebugBreakpoint* next;
 	uint32_t address;
+	bool isSw;
+	struct {
+		uint32_t opcode;
+		enum ExecutionMode mode;
+	} sw;
 };
 
 enum WatchpointType {
@@ -71,8 +76,11 @@ enum DebuggerState state;
 	struct ARMCore* cpu;
 
 	struct DebugBreakpoint* breakpoints;
+	struct DebugBreakpoint* swBreakpoints;
 	struct DebugWatchpoint* watchpoints;
 	struct ARMMemory originalMemory;
+
+	struct DebugBreakpoint* currentBreakpoint;
 
 	void (*init)(struct ARMDebugger*);
 	void (*deinit)(struct ARMDebugger*);
@@ -80,6 +88,9 @@ void (*paused)(struct ARMDebugger*);
 	void (*entered)(struct ARMDebugger*, enum DebuggerEntryReason, struct DebuggerEntryInfo*);
 	void (*custom)(struct ARMDebugger*);
 
+	bool (*setSoftwareBreakpoint)(struct ARMDebugger*, uint32_t address, enum ExecutionMode mode, uint32_t* opcode);
+	bool (*clearSoftwareBreakpoint)(struct ARMDebugger*, uint32_t address, enum ExecutionMode mode, uint32_t opcode);
+
 	__attribute__((format (printf, 3, 4)))
 	void (*log)(struct ARMDebugger*, enum DebuggerLogLevel, const char* format, ...);
 };
@@ -88,6 +99,7 @@ void ARMDebuggerCreate(struct ARMDebugger*);
 void ARMDebuggerRun(struct ARMDebugger*);
 void ARMDebuggerEnter(struct ARMDebugger*, enum DebuggerEntryReason, struct DebuggerEntryInfo*);
 void ARMDebuggerSetBreakpoint(struct ARMDebugger* debugger, uint32_t address);
+bool ARMDebuggerSetSoftwareBreakpoint(struct ARMDebugger* debugger, uint32_t address, enum ExecutionMode mode);
 void ARMDebuggerClearBreakpoint(struct ARMDebugger* debugger, uint32_t address);
 void ARMDebuggerSetWatchpoint(struct ARMDebugger* debugger, uint32_t address);
 void ARMDebuggerClearWatchpoint(struct ARMDebugger* debugger, uint32_t address);

@@ -31,6 +31,10 @@ static void GBAProcessEvents(struct ARMCore* cpu);
 static int32_t GBATimersProcessEvents(struct GBA* gba, int32_t cycles);
 static void GBAHitStub(struct ARMCore* cpu, uint32_t opcode);
 static void GBAIllegal(struct ARMCore* cpu, uint32_t opcode);
+static void GBABreakpoint(struct ARMCore* cpu, int immediate);
+
+static bool _setSoftwareBreakpoint(struct ARMDebugger*, uint32_t address, enum ExecutionMode mode, uint32_t* opcode);
+static bool _clearSoftwareBreakpoint(struct ARMDebugger*, uint32_t address, enum ExecutionMode mode, uint32_t opcode);
 
 void GBACreate(struct GBA* gba) {
 	gba->d.id = GBA_COMPONENT_MAGIC;
@@ -111,6 +115,8 @@ irqh->swi32 = GBASwi32;
 	irqh->hitIllegal = GBAIllegal;
 	irqh->readCPSR = GBATestIRQ;
 	irqh->hitStub = GBAHitStub;
+	irqh->bkpt16 = GBABreakpoint;
+	irqh->bkpt32 = GBABreakpoint;
 }
 
 void GBAReset(struct ARMCore* cpu) {
@@ -336,6 +342,8 @@ return nextEvent;
 }
 
 void GBAAttachDebugger(struct GBA* gba, struct ARMDebugger* debugger) {
+	debugger->setSoftwareBreakpoint = _setSoftwareBreakpoint;
+	debugger->clearSoftwareBreakpoint = _clearSoftwareBreakpoint;
 	gba->debugger = debugger;
 	gba->cpu->components[GBA_COMPONENT_DEBUGGER] = &debugger->d;
 	ARMHotplugAttach(gba->cpu, GBA_COMPONENT_DEBUGGER);
@@ -648,6 +656,25 @@ ARMDebuggerEnter(gba->debugger, DEBUGGER_ENTER_ILLEGAL_OP, &info);
 	}
 }
 
+void GBABreakpoint(struct ARMCore* cpu, int immediate) {
+	struct GBA* gba = (struct GBA*) cpu->master;
+	if (immediate >= GBA_COMPONENT_MAX) {
+		return;
+	}
+	switch (immediate) {
+	case GBA_COMPONENT_DEBUGGER:
+		if (gba->debugger) {
+			struct DebuggerEntryInfo info = {
+				.address = _ARMPCAddress(cpu)
+			};
+			ARMDebuggerEnter(gba->debugger, DEBUGGER_ENTER_BREAKPOINT, &info);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
 void GBAFrameStarted(struct GBA* gba) {
 	UNUSED(gba);
 
@@ -683,3 +710,68 @@ if (thread->frameCallback) {
 		thread->frameCallback(thread);
 	}
 }
+
+static bool _setSoftwareBreakpoint(struct ARMDebugger* debugger, uint32_t address, enum ExecutionMode mode, uint32_t* opcode) {
+	struct GBA* gba = (struct GBA*) debugger->cpu->master;
+
+	int immediate = GBA_COMPONENT_DEBUGGER;
+	uint32_t value;
+	if (mode == MODE_ARM) {
+		value = 0xE1200070;
+		value |= immediate & 0xF;
+		value |= (immediate & 0xFFF0) << 4;
+	} else {
+		value = 0xBE00;
+		value |= immediate & 0xFF;
+	}
+	uint32_t old;
+
+	switch (address >> BASE_OFFSET) {
+	case REGION_CART0:
+	case REGION_CART0_EX:
+	case REGION_CART1:
+	case REGION_CART1_EX:
+	case REGION_CART2:
+	case REGION_CART2_EX:
+		if ((address & (SIZE_CART0 - 1)) < gba->memory.romSize) {
+			if (mode == MODE_ARM) {
+				LOAD_32(old, address & (SIZE_CART0 - 1), gba->memory.rom);
+				STORE_32(value, address & (SIZE_CART0 - 1), gba->memory.rom);
+			} else {
+				LOAD_16(old, address & (SIZE_CART0 - 1), gba->memory.rom);
+				STORE_16(value, address & (SIZE_CART0 - 1), gba->memory.rom);
+			}
+			*opcode = old;
+			return true;
+		}
+		break;
+	default:
+		break;
+	}
+	return false;
+}
+
+static bool _clearSoftwareBreakpoint(struct ARMDebugger* debugger, uint32_t address, enum ExecutionMode mode, uint32_t opcode) {
+	struct GBA* gba = (struct GBA*) debugger->cpu->master;
+
+	switch (address >> BASE_OFFSET) {
+	case REGION_CART0:
+	case REGION_CART0_EX:
+	case REGION_CART1:
+	case REGION_CART1_EX:
+	case REGION_CART2:
+	case REGION_CART2_EX:
+		if ((address & (SIZE_CART0 - 1)) < gba->memory.romSize) {
+			if (mode == MODE_ARM) {
+				STORE_32(opcode, address & (SIZE_CART0 - 1), gba->memory.rom);
+			} else {
+				STORE_16(opcode, address & (SIZE_CART0 - 1), gba->memory.rom);
+			}
+			return true;
+		}
+		break;
+	default:
+		break;
+	}
+	return false;
+}