diff --git a/src/arm/isa-arm.c b/src/arm/isa-arm.c index 3a7d8d228..d0284b36e 100644 --- a/src/arm/isa-arm.c +++ b/src/arm/isa-arm.c @@ -355,27 +355,68 @@ void ARMStep(struct ARMCore* cpu) { 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 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 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, 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 @@ DEFINE_LOAD_STORE_T_INSTRUCTION_ARM(STRT, 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)