From 967215dcef72d77c5d111c258c9d5c5050dcf6ba Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Tue, 3 Jan 2017 22:39:37 -0800
Subject: [PATCH] ARM9: Implement BLX (2) Thumb

---
 include/mgba/internal/arm/emitter-thumb.h |  4 ++--
 src/arm/decoder-thumb.c                   |  6 ++++++
 src/arm/decoder.c                         |  7 +++++++
 src/arm/isa-thumb.c                       | 15 +++++++++++++++
 4 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/include/mgba/internal/arm/emitter-thumb.h b/include/mgba/internal/arm/emitter-thumb.h
index f2e69d0bb..670b8cde7 100644
--- a/include/mgba/internal/arm/emitter-thumb.h
+++ b/include/mgba/internal/arm/emitter-thumb.h
@@ -50,8 +50,8 @@
 	DECLARE_INSTRUCTION_WITH_HIGH_THUMB(EMITTER, MOV3), \
 	DECLARE_INSTRUCTION_THUMB(EMITTER, BX), \
 	DECLARE_INSTRUCTION_THUMB(EMITTER, BX), \
-	DECLARE_INSTRUCTION_THUMB(EMITTER, ILL), \
-	DECLARE_INSTRUCTION_THUMB(EMITTER, ILL), \
+	MIN_V(DECLARE_INSTRUCTION_THUMB(EMITTER, BLX2), DECLARE_INSTRUCTION_THUMB(EMITTER, ILL), V >= 5), \
+	MIN_V(DECLARE_INSTRUCTION_THUMB(EMITTER, BLX2), DECLARE_INSTRUCTION_THUMB(EMITTER, ILL), V >= 5), \
 	DO_8(DO_4(DECLARE_INSTRUCTION_THUMB(EMITTER, LDR3))), \
 	DO_8(DECLARE_INSTRUCTION_THUMB(EMITTER, STR2)), \
 	DO_8(DECLARE_INSTRUCTION_THUMB(EMITTER, STRH2)), \
diff --git a/src/arm/decoder-thumb.c b/src/arm/decoder-thumb.c
index bb89050a0..40ae39ed4 100644
--- a/src/arm/decoder-thumb.c
+++ b/src/arm/decoder-thumb.c
@@ -290,6 +290,12 @@ DEFINE_THUMB_DECODER(BLX1, BLX,
 		ARM_OPERAND_REGISTER_2 | ARM_OPERAND_IMMEDIATE_3;
 	info->branchType = ARM_BRANCH_LINKED;)
 
+DEFINE_THUMB_DECODER(BLX2, BLX,
+	info->op1.reg = (opcode >> 3) & 0xF;
+	info->op2.reg = ARM_LR;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 | ARM_OPERAND_REGISTER_2 | ARM_OPERAND_AFFECTED_2;
+	info->branchType = ARM_BRANCH_INDIRECT;)
+
 DEFINE_THUMB_DECODER(BX, BX,
 	info->op1.reg = (opcode >> 3) & 0xF;
 	info->operandFormat = ARM_OPERAND_REGISTER_1;
diff --git a/src/arm/decoder.c b/src/arm/decoder.c
index 8c7d25540..cd2324c87 100644
--- a/src/arm/decoder.c
+++ b/src/arm/decoder.c
@@ -393,6 +393,13 @@ int ARMDisassemble(struct ARMInstructionInfo* info, uint32_t pc, char* buffer, i
 		if (info->operandFormat & ARM_OPERAND_IMMEDIATE_1) {
 			written = _decodePCRelative(info->op1.immediate, pc, buffer, blen);
 			ADVANCE(written);
+		} else if (info->operandFormat & ARM_OPERAND_REGISTER_1) {
+			written = _decodeRegister(info->op1.reg, buffer, blen);
+			ADVANCE(written);
+			if (info->op1.reg > ARM_PC) {
+				written = _decodePSR(info->op1.psrBits, buffer, blen);
+				ADVANCE(written);
+			}
 		}
 		break;
 	default:
diff --git a/src/arm/isa-thumb.c b/src/arm/isa-thumb.c
index b65483d67..ed35e0a94 100644
--- a/src/arm/isa-thumb.c
+++ b/src/arm/isa-thumb.c
@@ -404,6 +404,21 @@ DEFINE_INSTRUCTION_THUMB(BX,
 		ARM_WRITE_PC;
 	})
 
+DEFINE_INSTRUCTION_THUMB(BLX2,
+	int rm = (opcode >> 3) & 0xF;
+	_ARMSetMode(cpu, cpu->gprs[rm] & 0x00000001);
+	int misalign = 0;
+	if (rm == ARM_PC) {
+		misalign = cpu->gprs[rm] & 0x00000002;
+	}
+	cpu->gprs[ARM_LR] = cpu->gprs[ARM_PC] - 1;
+	cpu->gprs[ARM_PC] = (cpu->gprs[rm] & 0xFFFFFFFE) - misalign;
+	if (cpu->executionMode == MODE_THUMB) {
+		THUMB_WRITE_PC;
+	} else {
+		ARM_WRITE_PC;
+	})
+
 DEFINE_INSTRUCTION_THUMB(SWI, cpu->irqh.swi16(cpu, opcode & 0xFF))
 
 const ThumbInstruction _thumbv4Table[0x400] = {