M src/arm/isa-arm.c → src/arm/isa-arm.c

@@ -355,27 +355,68 @@ DEFINE_LOAD_STORE_T_INSTRUCTION_SHIFTER_ARM(NAME ## _ROR_, ADDR_MODE_2_ROR, BODY) \
 	DEFINE_LOAD_STORE_INSTRUCTION_EX_ARM(NAME ## I, ADDR_MODE_2_RN, ADDR_MODE_2_WRITEBACK(ADDR_MODE_2_INDEX(-, ADDR_MODE_2_IMMEDIATE)), BODY) \
 	DEFINE_LOAD_STORE_INSTRUCTION_EX_ARM(NAME ## IU, ADDR_MODE_2_RN, ADDR_MODE_2_WRITEBACK(ADDR_MODE_2_INDEX(+, ADDR_MODE_2_IMMEDIATE)), BODY) \
 
-// TODO
-#define DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME, ADDRESS, S_PRE, S_POST, BODY) \
-	DEFINE_INSTRUCTION_ARM(NAME, BODY;)
+#define ARM_MS_PRE \
+	enum PrivilegeMode privilegeMode = cpu->privilegeMode; \
+	ARMSetPrivilegeMode(cpu, MODE_SYSTEM);
+
+#define ARM_MS_POST ARMSetPrivilegeMode(cpu, privilegeMode);
+
+#define ADDR_MODE_4_DA uint32_t addr = cpu->gprs[rn]
+#define ADDR_MODE_4_IA uint32_t addr = cpu->gprs[rn]
+#define ADDR_MODE_4_DB uint32_t addr = cpu->gprs[rn] - 4
+#define ADDR_MODE_4_IB uint32_t addr = cpu->gprs[rn] + 4
+#define ADDR_MODE_4_DAW cpu->gprs[rn] = addr
+#define ADDR_MODE_4_IAW cpu->gprs[rn] = addr
+#define ADDR_MODE_4_DBW cpu->gprs[rn] = addr + 4
+#define ADDR_MODE_4_IBW cpu->gprs[rn] = addr - 4
+
+#define ARM_M_INCREMENT(BODY) \
+	for (m = rs, i = 0; m; m >>= 1, ++i) { \
+		if (m & 1) { \
+			BODY; \
+			addr += 4; \
+		} \
+	}
+
+#define ARM_M_DECREMENT(BODY) \
+	for (m = 0x8000, i = 15; m; m >>= 1, --i) { \
+		if (rs & m) { \
+			BODY; \
+			addr -= 4; \
+		} \
+	}
+
+#define DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME, ADDRESS, WRITEBACK, LOOP, S_PRE, S_POST, BODY, POST_BODY) \
+	DEFINE_INSTRUCTION_ARM(NAME, \
+		int rn = (opcode >> 16) & 0xF; \
+		int rs = opcode & 0x0000FFFF; \
+		int m; \
+		int i; \
+		ADDRESS; \
+		S_PRE; \
+		LOOP(BODY); \
+		S_POST; \
+		WRITEBACK; \
+		POST_BODY;)
 
-#define DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_ARM(NAME, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DA, ADDR_MODE_4_DA, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DAW, ADDR_MODE_4_DAW, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DB, ADDR_MODE_4_DB, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DBW, ADDR_MODE_4_DBW, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IA, ADDR_MODE_4_IA, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IAW, ADDR_MODE_4_IAW, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IB, ADDR_MODE_4_IB, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IBW, ADDR_MODE_4_IBW, , , BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDA, ADDR_MODE_4_DA, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDAW, ADDR_MODE_4_DAW, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDB, ADDR_MODE_4_DB, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDBW, ADDR_MODE_4_DBW, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIA, ADDR_MODE_4_IA, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIAW, ADDR_MODE_4_IAW, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIB, ADDR_MODE_4_IB, S_PRE, S_POST, BODY) \
-	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIBW, ADDR_MODE_4_IBW, S_PRE, S_POST, BODY)
+
+#define DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_ARM(NAME, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DA,   ADDR_MODE_4_DA,                , ARM_M_DECREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DAW,  ADDR_MODE_4_DA, ADDR_MODE_4_DAW, ARM_M_DECREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DB,   ADDR_MODE_4_DB,                , ARM_M_DECREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## DBW,  ADDR_MODE_4_DB, ADDR_MODE_4_DBW, ARM_M_DECREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IA,   ADDR_MODE_4_IA,                , ARM_M_INCREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IAW,  ADDR_MODE_4_IA, ADDR_MODE_4_IAW, ARM_M_INCREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IB,   ADDR_MODE_4_IB,                , ARM_M_INCREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## IBW,  ADDR_MODE_4_IB, ADDR_MODE_4_IBW, ARM_M_INCREMENT, , , BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDA,  ADDR_MODE_4_DA,                , ARM_M_DECREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDAW, ADDR_MODE_4_DA, ADDR_MODE_4_DAW, ARM_M_DECREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDB,  ADDR_MODE_4_DB,                , ARM_M_DECREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SDBW, ADDR_MODE_4_DB, ADDR_MODE_4_DBW, ARM_M_DECREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIA,  ADDR_MODE_4_IA,                , ARM_M_INCREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIAW, ADDR_MODE_4_IA, ADDR_MODE_4_IAW, ARM_M_INCREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIB,  ADDR_MODE_4_IB,                , ARM_M_INCREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY) \
+	DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_EX_ARM(NAME ## SIBW, ADDR_MODE_4_IB, ADDR_MODE_4_IBW, ARM_M_INCREMENT, ARM_MS_PRE, ARM_MS_POST, BODY, POST_BODY)
 
 // Begin ALU definitions
 
@@ -484,8 +525,13 @@ ARMSetPrivilegeMode(cpu, MODE_USER);
 	cpu->memory->store8(cpu->memory, address, cpu->gprs[rd]);
 	ARMSetPrivilegeMode(cpu, priv);)
 
-DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_ARM(LDM, ARM_STUB)
-DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_ARM(STM, ARM_STUB)
+DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_ARM(LDM,
+	cpu->gprs[i] = cpu->memory->load32(cpu->memory, addr);,
+	if (rs & 0x8000) {
+		ARM_WRITE_PC;
+	})
+
+DEFINE_LOAD_STORE_MULTIPLE_INSTRUCTION_ARM(STM, cpu->memory->store32(cpu->memory, addr, cpu->gprs[i]);, )
 
 DEFINE_INSTRUCTION_ARM(SWP, ARM_STUB)
 DEFINE_INSTRUCTION_ARM(SWPB, ARM_STUB)