From 4f3c9a5e588bfbd7e8fdf5a929344f01c64e6dbc Mon Sep 17 00:00:00 2001 From: David Guillen Fandos Date: Wed, 11 Jan 2023 20:44:56 +0100 Subject: [PATCH] [all] Fix CPSR and CPU modes gpsp doesn't differentiate between USER and SYSTEM mode, most likely cause it is not that important for most games. This implements the modes correctly and adds checks for privileged operations. Still some bugs/hacks but it mostly fixes CPSR/SPSR reads/writes. To implement PSR writes we are using a more refined masks and force mode bit num. 4 to always be one. Reserved bits are forced to zero (this needs to be validated on a real device). --- arm/arm64_emit.h | 28 +++++++++----- arm/arm64_stub.S | 24 ++++++++---- arm/arm_emit.h | 23 +++++++----- arm/arm_stub.S | 27 +++++++++----- cpu.c | 96 ++++++++++++++++++++++++++---------------------- cpu.h | 26 ++++++++----- cpu_threaded.c | 4 +- mips/mips_emit.h | 30 ++++++++++----- mips/mips_stub.S | 27 ++++++++++---- x86/x86_emit.h | 31 +++++++++------- x86/x86_stub.S | 6 ++- 11 files changed, 197 insertions(+), 125 deletions(-) diff --git a/arm/arm64_emit.h b/arm/arm64_emit.h index e2dd391..4b4cb7a 100644 --- a/arm/arm64_emit.h +++ b/arm/arm64_emit.h @@ -645,12 +645,12 @@ u32 arm_to_a64_reg[] = u32 execute_spsr_restore_body(u32 address) { - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((reg[REG_CPSR] & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = address + 4; - spsr[MODE_IRQ] = reg[REG_CPSR]; + REG_MODE(MODE_IRQ)[6] = address + 4; + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; reg[REG_CPSR] = 0xD2; address = 0x00000018; set_cpu_mode(MODE_IRQ); @@ -1227,17 +1227,17 @@ u32 execute_spsr_restore_body(u32 address) generate_function_call(execute_read_##psr_reg); \ generate_store_reg(reg_res, rd) \ -u32 execute_store_cpsr_body(u32 _cpsr, u32 store_mask, u32 address) +u32 execute_store_cpsr_body(u32 _cpsr, u32 address, u32 store_mask) { reg[REG_CPSR] = _cpsr; if(store_mask & 0xFF) { - set_cpu_mode(cpu_modes[_cpsr & 0x1F]); + set_cpu_mode(cpu_modes[_cpsr & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((_cpsr & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = address + 4; - spsr[MODE_IRQ] = _cpsr; + REG_MODE(MODE_IRQ)[6] = address + 4; + REG_SPSR(MODE_IRQ) = _cpsr; reg[REG_CPSR] = 0xD2; set_cpu_mode(MODE_IRQ); return 0x00000018; @@ -1253,11 +1253,19 @@ u32 execute_store_cpsr_body(u32 _cpsr, u32 store_mask, u32 address) #define arm_psr_load_new_imm() \ generate_load_imm(reg_a0, imm) \ +#define arm_psr_store_cpsr(op_type) \ + generate_load_pc(reg_a1, (pc)); \ + generate_load_imm(reg_a2, cpsr_masks[psr_pfield][0]); \ + generate_load_imm(reg_temp, cpsr_masks[psr_pfield][1]); \ + generate_function_call(execute_store_cpsr) \ + +#define arm_psr_store_spsr(op_type) \ + generate_load_imm(reg_a1, spsr_masks[psr_pfield]); \ + generate_function_call(execute_store_spsr) \ + #define arm_psr_store(op_type, psr_reg) \ arm_psr_load_new_##op_type(); \ - generate_load_imm(reg_a1, psr_masks[psr_field]); \ - generate_load_pc(reg_a2, (pc)); \ - generate_function_call(execute_store_##psr_reg) \ + arm_psr_store_##psr_reg(op_type) #define arm_psr(op_type, transfer_type, psr_reg) \ { \ diff --git a/arm/arm64_stub.S b/arm/arm64_stub.S index 8b67296..fb66ece 100644 --- a/arm/arm64_stub.S +++ b/arm/arm64_stub.S @@ -89,7 +89,7 @@ _##symbol: #define PALCNV_RAM_OFF 0xE00 // Used for SWI handling -#define MODE_SUPERVISOR 3 +#define MODE_SUPERVISOR 0x13 #define SUPERVISOR_SPSR (SPSR_RAM_OFF + 3*4) // spsr[3] #define SUPERVISOR_LR (REGMODE_RAM_OFF + (3 * (7 * 4)) + (6 * 4)) // reg_mode[3][6] @@ -227,8 +227,9 @@ defsymbl(execute_read_cpsr) defsymbl(execute_read_spsr) ldr w1, [reg_base, #CPU_MODE] // read cpu mode to w1 + and w1, w1, #0xF // Like REG_SPSR() macro add x0, reg_base, #SPSR_RAM_OFF // ptr to spsr table - ldr w0, [x0, x1, lsl #2] // Read actual value from trable + ldr w0, [x0, x1, lsl #2] // Read actual value from table ret @@ -236,14 +237,19 @@ defsymbl(execute_read_spsr) // Input: // w0: new cpsr value -// w1: bitmask of which bits in cpsr to update -// w2: current PC +// w1: current PC +// w2: store bitmask (user-mode) +// w3: store bitmask (privileged mode) defsymbl(execute_store_cpsr) + ldr w4, [reg_base, #CPU_MODE] // w4 = cpu_mode + tst x4, #0x10 // Bit 4 is set on privileged modes + csel x2, x2, x3, eq // Select the correct mask + ldr w4, [reg_base, #REG_CPSR] // read current CPSR - and w3, w0, w1 // reg_flags = new_cpsr & store_mask - bic w4, w4, w1 // current_cpsr & ~store_mask - orr w0, w3, w4 // w3 = final CPSR value + and w3, w0, w2 // reg_flags = new_cpsr & store_mask + bic w4, w4, w2 // current_cpsr & ~store_mask + orr w0, w3, w4 // w2 = final CPSR value extract_flags_reg(w0) // Update cached flags too str lr, [reg_base, #REG_SAVE] @@ -270,6 +276,7 @@ defsymbl(execute_store_cpsr) defsymbl(execute_store_spsr) ldr w2, [reg_base, #CPU_MODE] // read cpu mode to w1 + and w2, w2, #0xF // Like REG_SPSR() macro add x2, reg_base, x2, lsl #2 // calculate table offset ldr w3, [x2, #SPSR_RAM_OFF] // Read actual value from trable @@ -288,7 +295,8 @@ defsymbl(execute_store_spsr) defsymbl(execute_spsr_restore) ldr w1, [reg_base, #CPU_MODE] // w1 = cpu_mode - cbz w1, 1f // Ignore if in user mode + and w1, w1, 0xF // Fold user and system modes + cbz w1, 1f // Ignore if in user or system mode lsl w2, w1, #2 // We access 32 bit words add w2, w2, #SPSR_RAM_OFF diff --git a/arm/arm_emit.h b/arm/arm_emit.h index 68d83fa..0b08a8d 100644 --- a/arm/arm_emit.h +++ b/arm/arm_emit.h @@ -44,7 +44,7 @@ void arm_indirect_branch_thumb(u32 address); void arm_indirect_branch_dual_arm(u32 address); void arm_indirect_branch_dual_thumb(u32 address); -void execute_store_cpsr(u32 new_cpsr, u32 store_mask, u32 address); +void execute_store_cpsr(u32 new_cpsr); u32 execute_store_cpsr_body(u32 _cpsr, u32 store_mask, u32 address); u32 execute_spsr_restore(u32 address); @@ -691,8 +691,8 @@ u32 thumb_prepare_load_reg_pc(u8 **tptr, u32 scratch_reg, u32 reg_index, u32 pc_ if((io_registers[REG_IE] & io_registers[REG_IF]) && \ io_registers[REG_IME] && ((reg[REG_CPSR] & 0x80) == 0)) \ { \ - reg_mode[MODE_IRQ][6] = pc + 4; \ - spsr[MODE_IRQ] = reg[REG_CPSR]; \ + REG_MODE(MODE_IRQ)[6] = pc + 4; \ + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; \ reg[REG_CPSR] = 0xD2; \ pc = 0x00000018; \ set_cpu_mode(MODE_IRQ); \ @@ -708,7 +708,7 @@ u32 thumb_prepare_load_reg_pc(u8 **tptr, u32 scratch_reg, u32 reg_index, u32 pc_ u32 execute_spsr_restore_body(u32 pc) { - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); check_for_interrupts(); return pc; @@ -1211,6 +1211,7 @@ u32 execute_spsr_restore_body(u32 pc) u32 _rd = arm_prepare_store_reg(reg_a0, rd); \ ARM_ADD_REG_IMM(0, reg_a0, reg_base, SPSR_RAM_OFF >> 2, 30); \ ARM_LDR_IMM(0, reg_a1, reg_base, CPU_MODE * 4); \ + ARM_AND_REG_IMM(0, reg_a1, reg_a1, 0xF, 0); \ ARM_LDR_REG_REG_SHIFT(0, _rd, reg_a0, reg_a1, ARMSHIFT_LSL, 2); \ arm_complete_store_reg(_rd, rd); \ } @@ -1227,12 +1228,12 @@ u32 execute_store_cpsr_body(u32 _cpsr, u32 store_mask, u32 address) reg[REG_CPSR] = _cpsr; if(store_mask & 0xFF) { - set_cpu_mode(cpu_modes[_cpsr & 0x1F]); + set_cpu_mode(cpu_modes[_cpsr & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((_cpsr & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = address + 4; - spsr[MODE_IRQ] = _cpsr; + REG_MODE(MODE_IRQ)[6] = address + 4; + REG_SPSR(MODE_IRQ) = _cpsr; reg[REG_CPSR] = 0xD2; set_cpu_mode(MODE_IRQ); return 0x00000018; @@ -1288,13 +1289,15 @@ static void trace_instruction(u32 pc, u32 mode) generate_load_imm(reg_a0, imm, imm_ror) \ #define arm_psr_store_cpsr() \ - arm_load_imm_32bit(reg_a1, psr_masks[psr_field]); \ generate_function_far_call(armfn_store_cpsr); \ - write32(pc) \ + write32(cpsr_masks[psr_pfield][0]); \ + write32(cpsr_masks[psr_pfield][1]); \ + write32(pc); \ #define arm_psr_store_spsr() \ - arm_load_imm_32bit(reg_a1, psr_masks[psr_field]); \ + arm_load_imm_32bit(reg_a1, spsr_masks[psr_pfield]); \ ARM_LDR_IMM(0, reg_a2, reg_base, (CPU_MODE * 4)); \ + ARM_AND_REG_IMM(0, reg_a2, reg_a2, 0xF, 0); \ ARM_ADD_REG_IMMSHIFT(0, ARMREG_LR, reg_base, reg_a2, ARMSHIFT_LSL, 2); \ ARM_AND_REG_IMMSHIFT(0, reg_a0, reg_a0, reg_a1, ARMSHIFT_LSL, 0); \ ARM_LDR_IMM(0, reg_a2, ARMREG_LR, SPSR_RAM_OFF); \ diff --git a/arm/arm_stub.S b/arm/arm_stub.S index 0574845..2248bdb 100644 --- a/arm/arm_stub.S +++ b/arm/arm_stub.S @@ -65,7 +65,8 @@ _##symbol: #define reg_x5 r8 -#define MODE_SUPERVISOR 3 +#define MODE_SUPERVISOR 0x13 +#define SUPERVISOR_OFFSET 0x03 @ Memory offsets from reg_base to the different buffers #define IWRAM_OFF -0xA8000 @@ -301,11 +302,16 @@ defsymbl(arm_indirect_branch_dual_thumb) @ Input: @ r0: new cpsr value -@ r1: bitmask of which bits in cpsr to update -@ r2: current PC +@ [lr]: bitmask (user mode) +@ [lr+4]: bitmask (privileged mode) +@ [lr+8]: current PC defsymbl(execute_store_cpsr) save_flags() + ldr r1, [reg_base, #CPU_MODE] @ r1 = cpu_mode + lsr r1, r1, #4 @ Load privilege bit + ldr r1, [lr, r1, lsl #2] @ Load mask + and reg_flags, r0, r1 @ reg_flags = new_cpsr & store_mask ldr r0, [reg_base, #REG_CPSR] @ r0 = cpsr bic r0, r0, r1 @ r0 = cpsr & ~store_mask @@ -314,7 +320,7 @@ defsymbl(execute_store_cpsr) mov r0, reg_flags @ also put new cpsr in r0 store_registers_arm() @ save ARM registers - ldr r2, [lr] @ r2 = pc + ldr r2, [lr, #8] @ r2 = pc call_c_function(execute_store_cpsr_body) load_registers_arm() @ restore ARM registers @@ -322,7 +328,7 @@ defsymbl(execute_store_cpsr) bne 1f @ if it's zero, resume restore_flags() - add pc, lr, #4 @ return + add pc, lr, #12 @ return (skip data) 1: call_c_function(block_lookup_address_arm) @@ -338,8 +344,9 @@ defsymbl(execute_store_cpsr) defsymbl(execute_spsr_restore) save_flags() ldr r2, [reg_base, #CPU_MODE] @ r2 = cpu_mode - cmp r2, #0 @ if usermode already, do not change - beq 2f @ just a regular indirect jump really + ands r2, r2, #0xF @ Ignore privilege bits + beq 2f @ if user/system mode no side effects + add r1, reg_base, #SPSR_RAM_OFF @ r1 = spsr ldr r1, [r1, r2, lsl #2] @ r1 = spsr[cpu_mode] (new cpsr) str r1, [reg_base, #REG_CPSR] @ update cpsr @@ -376,12 +383,12 @@ defsymbl(execute_spsr_restore) defsymbl(execute_swi_##mode) ;\ save_flags() ;\ add r1, reg_base, #REGMODE_RAM_OFF /* r1 = reg_mode */;\ - /* reg_mode[MODE_SUPERVISOR][6] = pc */;\ + /* REG_MODE(MODE_SUPERVISOR)[6] = pc */;\ ldr r0, [lr] /* load PC */;\ - str r0, [r1, #((MODE_SUPERVISOR * (7 * 4)) + (6 * 4))] ;\ + str r0, [r1, #((SUPERVISOR_OFFSET * (7 * 4)) + (6 * 4))] ;\ collapse_flags_no_update(r0) /* r0 = cpsr */;\ add r1, reg_base, #SPSR_RAM_OFF /* r1 = spsr */;\ - str r0, [r1, #(MODE_SUPERVISOR * 4)] /* spsr[MODE_SUPERVISOR] = cpsr */;\ + str r0, [r1, #(SUPERVISOR_OFFSET * 4)] /* spsr[MODE_SUPERVISOR] = cpsr */;\ bic r0, r0, #0x3F /* clear mode flag in r0 */;\ orr r0, r0, #(0x13 | 0x80) /* supervisor mode + disable IRQ */;\ str r0, [reg_base, #REG_CPSR] /* update cpsr */;\ diff --git a/cpu.c b/cpu.c index 8b77fad..c709eba 100644 --- a/cpu.c +++ b/cpu.c @@ -181,18 +181,18 @@ void print_register_usage(void) using_register(arm, rn, op_src) \ #define arm_decode_psr_reg(opcode) \ - u32 psr_field = (opcode >> 16) & 0x0F; \ + u32 psr_pfield = ((opcode >> 16) & 1) | ((opcode >> 18) & 2); \ u32 rd = (opcode >> 12) & 0x0F; \ u32 rm = opcode & 0x0F; \ (void)rd; \ (void)rm; \ - (void)psr_field; \ + (void)psr_pfield; \ using_register(arm, rd, op_dest); \ using_register(arm, rm, op_src) \ #define arm_decode_psr_imm(opcode) \ u32 imm; \ - u32 psr_field = (opcode >> 16) & 0x0F; \ + u32 psr_pfield = ((opcode >> 16) & 1) | ((opcode >> 18) & 2); \ u32 rd = (opcode >> 12) & 0x0F; \ (void)rd; \ ror(imm, opcode & 0xFF, ((opcode >> 8) & 0x0F) * 2); \ @@ -718,8 +718,8 @@ void print_register_usage(void) if((read_ioreg(REG_IE) & read_ioreg(REG_IF)) && \ read_ioreg(REG_IME) && ((reg[REG_CPSR] & 0x80) == 0)) \ { \ - reg_mode[MODE_IRQ][6] = reg[REG_PC] + 4; \ - spsr[MODE_IRQ] = reg[REG_CPSR]; \ + REG_MODE(MODE_IRQ)[6] = reg[REG_PC] + 4; \ + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; \ reg[REG_CPSR] = 0xD2; \ reg[REG_PC] = 0x00000018; \ arm_update_pc(); \ @@ -730,11 +730,11 @@ void print_register_usage(void) #define arm_spsr_restore() \ if(rd == 15) \ { \ - if(reg[CPU_MODE] != MODE_USER) \ + if(reg[CPU_MODE] != MODE_USER && reg[CPU_MODE] != MODE_SYSTEM) \ { \ - reg[REG_CPSR] = spsr[reg[CPU_MODE]]; \ + reg[REG_CPSR] = REG_SPSR(reg[CPU_MODE]); \ extract_flags(); \ - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); \ + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); \ check_for_interrupts(); \ } \ arm_update_pc(); \ @@ -890,33 +890,42 @@ void print_register_usage(void) arm_pc_offset(4); \ } \ -const u32 psr_masks[16] = +// Index by PRS fields (1 and 4 only!) and User-Privileged mode +// In user mode some bits are read only +// Bit #4 is always set to one (so all modes are 1XXXX) +// Reserved bits are always zero and cannot be modified +const u32 cpsr_masks[4][2] = { - 0x00000000, 0x000000FF, 0x0000FF00, 0x0000FFFF, 0x00FF0000, - 0x00FF00FF, 0x00FFFF00, 0x00FFFFFF, 0xFF000000, 0xFF0000FF, - 0xFF00FF00, 0xFF00FFFF, 0xFFFF0000, 0xFFFF00FF, 0xFFFFFF00, - 0xFFFFFFFF + // User, Privileged + {0x00000000, 0x00000000}, + {0x00000020, 0x000000EF}, + {0xF0000000, 0xF0000000}, + {0xF0000020, 0xF00000EF} }; +// SPSR is always a privileged instruction +const u32 spsr_masks[4] = { 0x00000000, 0x000000EF, 0xF0000000, 0xF00000EF }; + #define arm_psr_read(dummy, psr_reg) \ collapse_flags(); \ reg[rd] = psr_reg \ #define arm_psr_store_cpsr(source) \ + const u32 store_mask = cpsr_masks[psr_pfield][PRIVMODE(reg[CPU_MODE])]; \ reg[REG_CPSR] = (source & store_mask) | (reg[REG_CPSR] & (~store_mask)); \ extract_flags(); \ if(store_mask & 0xFF) \ { \ - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); \ + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); \ check_for_interrupts(); \ } \ #define arm_psr_store_spsr(source) \ - u32 _psr = spsr[reg[CPU_MODE]]; \ - spsr[reg[CPU_MODE]] = (source & store_mask) | (_psr & (~store_mask)) \ + const u32 store_mask = spsr_masks[psr_pfield]; \ + u32 _psr = REG_SPSR(reg[CPU_MODE]); \ + REG_SPSR(reg[CPU_MODE]) = (source & store_mask) | (_psr & (~store_mask)) \ #define arm_psr_store(source, psr_reg) \ - const u32 store_mask = psr_masks[psr_field]; \ arm_psr_store_##psr_reg(source) \ #define arm_psr_src_reg reg[rm] @@ -1518,15 +1527,12 @@ const u32 psr_masks[16] = // reg_mode[new_mode][6]. When swapping to/from FIQ retire/load reg[8] // through reg[14] to/from reg_mode[MODE_FIQ][0] through reg_mode[MODE_FIQ][6]. -const u32 cpu_modes[32] = +const u32 cpu_modes[16] = { - MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, - MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, - MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, - MODE_INVALID, MODE_USER, MODE_FIQ, MODE_IRQ, MODE_SUPERVISOR, MODE_INVALID, - MODE_INVALID, MODE_INVALID, MODE_ABORT, MODE_INVALID, MODE_INVALID, - MODE_INVALID, MODE_INVALID, MODE_UNDEFINED, MODE_INVALID, MODE_INVALID, - MODE_USER + MODE_USER, MODE_FIQ, MODE_IRQ, MODE_SUPERVISOR, + MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_ABORT, + MODE_INVALID, MODE_INVALID, MODE_INVALID, MODE_INVALID, + MODE_UNDEFINED, MODE_INVALID, MODE_INVALID, MODE_SYSTEM }; // ARM/Thumb mode is stored in the flags directly, this is simpler than @@ -1545,23 +1551,23 @@ void set_cpu_mode(cpu_mode_type new_mode) if(new_mode == MODE_FIQ) { for(i = 8; i < 15; i++) - reg_mode[cpu_mode][i - 8] = reg[i]; + REG_MODE(cpu_mode)[i - 8] = reg[i]; } else { - reg_mode[cpu_mode][5] = reg[REG_SP]; - reg_mode[cpu_mode][6] = reg[REG_LR]; + REG_MODE(cpu_mode)[5] = reg[REG_SP]; + REG_MODE(cpu_mode)[6] = reg[REG_LR]; } if(cpu_mode == MODE_FIQ) { for(i = 8; i < 15; i++) - reg[i] = reg_mode[new_mode][i - 8]; + reg[i] = REG_MODE(new_mode)[i - 8]; } else { - reg[REG_SP] = reg_mode[new_mode][5]; - reg[REG_LR] = reg_mode[new_mode][6]; + reg[REG_SP] = REG_MODE(new_mode)[5]; + reg[REG_LR] = REG_MODE(new_mode)[6]; } reg[CPU_MODE] = new_mode; @@ -1580,8 +1586,8 @@ void raise_interrupt(irq_type irq_raised) reg[REG_BUS_VALUE] = 0xe55ec002; // Interrupt handler in BIOS - reg_mode[MODE_IRQ][6] = reg[REG_PC] + 4; - spsr[MODE_IRQ] = reg[REG_CPSR]; + REG_MODE(MODE_IRQ)[6] = reg[REG_PC] + 4; + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; reg[REG_CPSR] = 0xD2; reg[REG_PC] = 0x00000018; @@ -2295,7 +2301,7 @@ arm_loop: else { /* MRS rd, spsr */ - arm_psr(reg, read, spsr[reg[CPU_MODE]]); + arm_psr(reg, read, REG_SPSR(reg[CPU_MODE])); } break; @@ -3198,9 +3204,9 @@ arm_loop: default: // After SWI, we read bios[0xE4] reg[REG_BUS_VALUE] = 0xe3a02004; - reg_mode[MODE_SUPERVISOR][6] = pc + 4; + REG_MODE(MODE_SUPERVISOR)[6] = pc + 4; collapse_flags(); - spsr[MODE_SUPERVISOR] = reg[REG_CPSR]; + REG_SPSR(MODE_SUPERVISOR) = reg[REG_CPSR]; reg[REG_PC] = 0x00000008; arm_update_pc(); // Move to ARM mode, Supervisor mode and disable IRQs @@ -3685,8 +3691,8 @@ thumb_loop: default: // After SWI, we read bios[0xE4] reg[REG_BUS_VALUE] = 0xe3a02004; - reg_mode[MODE_SUPERVISOR][6] = pc + 2; - spsr[MODE_SUPERVISOR] = reg[REG_CPSR]; + REG_MODE(MODE_SUPERVISOR)[6] = pc + 2; + REG_SPSR(MODE_SUPERVISOR) = reg[REG_CPSR]; reg[REG_PC] = 0x00000008; thumb_update_pc(); // Move to ARM mode, Supervisor mode and disable IRQs @@ -3760,9 +3766,11 @@ thumb_loop: void init_cpu(void) { // Initialize CPU registers + int i; memset(reg, 0, REG_USERDEF * sizeof(u32)); memset(reg_mode, 0, sizeof(reg_mode)); - memset(spsr, 0, sizeof(spsr)); + for (i = 0; i < sizeof(spsr)/sizeof(spsr[0]); i++) + spsr[i] = 0x00000010; reg[CPU_HALT_STATE] = CPU_ACTIVE; reg[CHANGED_PC_STATUS] = 0; @@ -3771,7 +3779,7 @@ void init_cpu(void) reg[REG_SP] = 0x03007F00; reg[REG_PC] = 0x08000000; reg[REG_CPSR] = 0x0000001F; // system mode - reg[CPU_MODE] = MODE_USER; + reg[CPU_MODE] = MODE_SYSTEM; } else { reg[REG_SP] = 0x03007F00; reg[REG_PC] = 0x00000000; @@ -3781,10 +3789,10 @@ void init_cpu(void) // Stack pointers are set by BIOS, we set them // nevertheless, should we not boot from BIOS - reg_mode[MODE_USER][5] = 0x03007F00; - reg_mode[MODE_IRQ][5] = 0x03007FA0; - reg_mode[MODE_FIQ][5] = 0x03007FA0; - reg_mode[MODE_SUPERVISOR][5] = 0x03007FE0; + REG_MODE(MODE_USER)[5] = 0x03007F00; + REG_MODE(MODE_IRQ)[5] = 0x03007FA0; + REG_MODE(MODE_FIQ)[5] = 0x03007FA0; + REG_MODE(MODE_SUPERVISOR)[5] = 0x03007FE0; } bool cpu_read_savestate(const u8 *src) diff --git a/cpu.h b/cpu.h index 0911ed4..1c641c3 100644 --- a/cpu.h +++ b/cpu.h @@ -27,13 +27,20 @@ typedef u32 cpu_mode_type; -#define MODE_USER 0x0 -#define MODE_IRQ 0x1 -#define MODE_FIQ 0x2 -#define MODE_SUPERVISOR 0x3 -#define MODE_ABORT 0x4 -#define MODE_UNDEFINED 0x5 -#define MODE_INVALID 0x6 +// Bit 4 indicates privilege level +#define MODE_USER 0x00 // Non-privileged mode +#define MODE_SYSTEM 0x10 // Privileged modes +#define MODE_IRQ 0x11 +#define MODE_FIQ 0x12 +#define MODE_SUPERVISOR 0x13 +#define MODE_ABORT 0x14 +#define MODE_UNDEFINED 0x15 +#define MODE_INVALID 0x16 + +// Discards privilege bit +#define REG_MODE(m) (reg_mode[(m) & 0xF]) +#define REG_SPSR(m) (spsr[(m) & 0xF]) +#define PRIVMODE(m) ((m) >> 4) #define CPU_ACTIVE 0 #define CPU_HALT 1 @@ -163,8 +170,9 @@ void init_bios_hooks(void); extern u32 reg_mode[7][7]; extern u32 spsr[6]; -extern const u32 cpu_modes[32]; -extern const u32 psr_masks[16]; +extern const u32 cpu_modes[16]; +extern const u32 cpsr_masks[4][2]; +extern const u32 spsr_masks[4]; extern u32 memory_region_access_read_u8[16]; extern u32 memory_region_access_read_s8[16]; diff --git a/cpu_threaded.c b/cpu_threaded.c index 78e367d..0816ea4 100644 --- a/cpu_threaded.c +++ b/cpu_threaded.c @@ -108,12 +108,12 @@ extern u8 bit_count[256]; u32 imm_ror = ((opcode >> 8) & 0x0F) * 2 \ #define arm_decode_psr_reg(opcode) \ - u32 psr_field = (opcode >> 16) & 0x0F; \ + u32 psr_pfield = ((opcode >> 16) & 1) | ((opcode >> 18) & 2); \ u32 rd = (opcode >> 12) & 0x0F; \ u32 rm = opcode & 0x0F \ #define arm_decode_psr_imm(opcode) \ - u32 psr_field = (opcode >> 16) & 0x0F; \ + u32 psr_pfield = ((opcode >> 16) & 1) | ((opcode >> 18) & 2); \ u32 rd = (opcode >> 12) & 0x0F; \ u32 imm = opcode & 0xFF; \ u32 imm_ror = ((opcode >> 8) & 0x0F) * 2 \ diff --git a/mips/mips_emit.h b/mips/mips_emit.h index 585edac..f9ce27c 100644 --- a/mips/mips_emit.h +++ b/mips/mips_emit.h @@ -181,6 +181,10 @@ u32 arm_to_mips_reg[] = mips_emit_jal(mips_absolute_offset(function_location)); \ mips_emit_nop() \ +#define generate_raw_u32(value) \ + *((u32 *)translation_ptr) = (value); \ + translation_ptr += 4 \ + #define generate_function_call_swap_delay(function_location) \ { \ u32 delay_instruction = address32(translation_ptr, -4); \ @@ -619,12 +623,12 @@ u32 generate_load_rm_sh_##flags_op(u32 rm) \ u32 execute_spsr_restore_body(u32 address) { - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((reg[REG_CPSR] & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = address + 4; - spsr[MODE_IRQ] = reg[REG_CPSR]; + REG_MODE(MODE_IRQ)[6] = address + 4; + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; reg[REG_CPSR] = 0xD2; address = 0x00000018; set_cpu_mode(MODE_IRQ); @@ -1164,12 +1168,12 @@ u32 execute_spsr_restore_body(u32 address) u32 execute_store_cpsr_body(u32 _cpsr, u32 address) { - set_cpu_mode(cpu_modes[_cpsr & 0x1F]); + set_cpu_mode(cpu_modes[_cpsr & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((_cpsr & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = address + 4; - spsr[MODE_IRQ] = _cpsr; + REG_MODE(MODE_IRQ)[6] = address + 4; + REG_SPSR(MODE_IRQ) = _cpsr; reg[REG_CPSR] = 0xD2; set_cpu_mode(MODE_IRQ); return 0x00000018; @@ -1184,11 +1188,19 @@ u32 execute_store_cpsr_body(u32 _cpsr, u32 address) #define arm_psr_load_new_imm() \ generate_load_imm(reg_a0, imm) \ +#define arm_psr_store_spsr() \ + generate_load_imm(reg_a1, spsr_masks[psr_pfield]); \ + generate_function_call_swap_delay(execute_store_spsr) \ + +#define arm_psr_store_cpsr() \ + generate_load_pc(reg_a1, (pc)); \ + generate_function_call_swap_delay(execute_store_cpsr); \ + generate_raw_u32(cpsr_masks[psr_pfield][0]); \ + generate_raw_u32(cpsr_masks[psr_pfield][1]); \ + #define arm_psr_store(op_type, psr_reg) \ arm_psr_load_new_##op_type(); \ - generate_load_pc(reg_a1, (pc)); \ - generate_load_imm(reg_a2, psr_masks[psr_field]); \ - generate_function_call_swap_delay(execute_store_##psr_reg) \ + arm_psr_store_##psr_reg(); \ #define arm_psr(op_type, transfer_type, psr_reg) \ { \ diff --git a/mips/mips_stub.S b/mips/mips_stub.S index b949276..8e41b3d 100644 --- a/mips/mips_stub.S +++ b/mips/mips_stub.S @@ -115,6 +115,7 @@ symbol: .equ SPSR_BASE, (0x100 + 0x400 * 3) .equ REGMODE_BASE, (SPSR_BASE + 24) +.equ SUPERVISOR_MODE, (0x13) .equ SUPERVISOR_SPSR, (3 * 4 + SPSR_BASE) .equ SUPERVISOR_LR, ((3 * (7 * 4)) + (6 * 4) + REGMODE_BASE) .equ FNPTRS_MEMOPS, (REGMODE_BASE + 196) @@ -399,6 +400,7 @@ defsymbl(execute_read_cpsr) defsymbl(execute_read_spsr) lw $1, CPU_MODE($16) # $1 = cpu_mode + andi $1, $1, 0xF # remove privilege bits sll $1, $1, 2 # adjust to word offset size addu $2, $1, $16 jr $ra # return @@ -417,7 +419,7 @@ defsymbl(execute_swi) ori $2, (0x13 | 0x80) # mode supervisor + disable IRQs sw $2, REG_CPSR($16) # write back CPSR save_registers - li $4, 3 # 3 is supervisor mode + li $4, SUPERVISOR_MODE cfncall set_cpu_mode, 5 # set the CPU mode to supervisor lw $ra, REG_SAVE3($16) restore_registers @@ -430,8 +432,9 @@ defsymbl(execute_swi) defsymbl(execute_spsr_restore) lw $1, CPU_MODE($16) # $1 = cpu_mode + andi $1, $1, 0xF # Remove privilege bits - beq $1, $0, no_spsr_restore # only restore if the cpu isn't usermode + beq $1, $0, no_spsr_restore # only restore if the cpu isn't user/sys mode sll $2, $1, 2 # adjust to word offset size (delay) addu $2, $2, $16 @@ -452,9 +455,17 @@ no_spsr_restore: # $4: new cpsr # $5: current PC -# $6: store mask +# [ra]: store mask (user) +# [ra+4]: store mask (privileged) defsymbl(execute_store_cpsr) + lw $6, CPU_MODE($16) # $6 = cpu_mode + andi $6, $6, 0x10 # Get privilege bit + srl $6, $6, 2 # Calculate offset + addu $6, $6, $ra + lw $6, ($6) # Load mask + addu $ra, $ra, 8 # Real return address + and $1, $4, $6 # $1 = new_cpsr & store_mask lw $2, REG_CPSR($16) # $2 = current cpsr nor $4, $6, $0 # $4 = ~store_mask @@ -492,17 +503,17 @@ defsymbl(execute_store_cpsr) # $4: new spsr -# $5: current PC (unused) -# $6: store mask +# $5: store mask defsymbl(execute_store_spsr) lw $1, CPU_MODE($16) # $1 = cpu_mode + andi $1, $1, 0xF # Remove privilege bits sll $1, $1, 2 # adjust to word offset size addu $1, $1, $16 lw $2, SPSR_BASE($1) # $2 = spsr[cpu_mode] - and $4, $4, $6 # $4 = new_spsr & store_mask - nor $6, $6, $0 # $6 = ~store_mask - and $2, $2, $6 # $2 = (spsr & (~store_mask)) + and $4, $4, $5 # $4 = new_spsr & store_mask + nor $5, $5, $0 # $5 = ~store_mask + and $2, $2, $5 # $2 = (spsr & (~store_mask)) or $4, $4, $2 # $4 = new spsr combined with old jr $ra # return sw $4, SPSR_BASE($1) # spsr[cpu_mode] = $4 (delay slot) diff --git a/x86/x86_emit.h b/x86/x86_emit.h index afff8c8..0df117e 100644 --- a/x86/x86_emit.h +++ b/x86/x86_emit.h @@ -1042,17 +1042,17 @@ typedef enum u32 function_cc execute_spsr_restore(u32 address) { - if(reg[CPU_MODE] != MODE_USER) + if(reg[CPU_MODE] != MODE_USER && reg[CPU_MODE] != MODE_SYSTEM) { - reg[REG_CPSR] = spsr[reg[CPU_MODE]]; + reg[REG_CPSR] = REG_SPSR(reg[CPU_MODE]); extract_flags(); - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((reg[REG_CPSR] & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = reg[REG_PC] + 4; - spsr[MODE_IRQ] = reg[REG_CPSR]; + REG_MODE(MODE_IRQ)[6] = reg[REG_PC] + 4; + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; reg[REG_CPSR] = 0xD2; address = 0x00000018; set_cpu_mode(MODE_IRQ); @@ -1348,6 +1348,7 @@ u32 function_cc execute_spsr_restore(u32 address) #define execute_read_spsr(oreg) \ collapse_flags(oreg, a2); \ generate_load_reg(oreg, CPU_MODE); \ + generate_and_imm(oreg, 0xF); \ generate_load_spsr(oreg, oreg); \ #define arm_psr_read(op_type, psr_reg) \ @@ -1357,12 +1358,12 @@ u32 function_cc execute_spsr_restore(u32 address) // Does mode-change magic (including IRQ checks) u32 execute_store_cpsr_body() { - set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0x1F]); + set_cpu_mode(cpu_modes[reg[REG_CPSR] & 0xF]); if((io_registers[REG_IE] & io_registers[REG_IF]) && io_registers[REG_IME] && ((reg[REG_CPSR] & 0x80) == 0)) { - reg_mode[MODE_IRQ][6] = reg[REG_PC] + 4; - spsr[MODE_IRQ] = reg[REG_CPSR]; + REG_MODE(MODE_IRQ)[6] = reg[REG_PC] + 4; + REG_SPSR(MODE_IRQ) = reg[REG_CPSR]; reg[REG_CPSR] = (reg[REG_CPSR] & 0xFFFFFF00) | 0xD2; set_cpu_mode(MODE_IRQ); return 0x00000018; @@ -1380,16 +1381,18 @@ u32 execute_store_cpsr_body() generate_load_imm(a0, imm) \ #define execute_store_cpsr() \ - generate_load_imm(a1, psr_masks[psr_field]); \ + generate_load_imm(a1, cpsr_masks[psr_pfield][0]); \ + generate_load_imm(a2, cpsr_masks[psr_pfield][1]); \ generate_store_reg_i32(pc, REG_PC); \ generate_function_call(execute_store_cpsr) \ -/* spsr[reg[CPU_MODE]] = (new_spsr & store_mask) | (old_spsr & (~store_mask))*/ +/* REG_SPSR(reg[CPU_MODE]) = (new_spsr & store_mask) | (old_spsr & (~store_mask))*/ #define execute_store_spsr() \ generate_load_reg(a2, CPU_MODE); \ + generate_and_imm(a2, 0xF); \ generate_load_spsr(a1, a2); \ - generate_and_imm(a0, psr_masks[psr_field]); \ - generate_and_imm(a1, ~psr_masks[psr_field]); \ + generate_and_imm(a0, spsr_masks[psr_pfield]); \ + generate_and_imm(a1, ~spsr_masks[psr_pfield]); \ generate_or(a0, a1); \ generate_store_spsr(a0, a2); \ @@ -2135,8 +2138,8 @@ static void function_cc execute_swi(u32 pc) { // Open bus value after SWI reg[REG_BUS_VALUE] = 0xe3a02004; - reg_mode[MODE_SUPERVISOR][6] = pc; - spsr[MODE_SUPERVISOR] = reg[REG_CPSR]; + REG_MODE(MODE_SUPERVISOR)[6] = pc; + REG_SPSR(MODE_SUPERVISOR) = reg[REG_CPSR]; // Move to ARM mode, supervisor mode, disable IRQs reg[REG_CPSR] = (reg[REG_CPSR] & ~0x3F) | 0x13 | 0x80; set_cpu_mode(MODE_SUPERVISOR); diff --git a/x86/x86_stub.S b/x86/x86_stub.S index 0f38edc..57c4eab 100644 --- a/x86/x86_stub.S +++ b/x86/x86_stub.S @@ -445,8 +445,12 @@ load_stubs( s8, movsbl, ~0, 0, read_memory8s) # arg0 (%eax) = new_cpsr -# arg1 (%edx) = store_mask +# arg1 (%edx) = store_mask (user mode) +# arg2 (%ecx) = store_mask (system mode) defsymbl(execute_store_cpsr) + testl $0x10, CPU_MODE(REG_BASE) # check privileged mode bit + cmovne %ecx, %edx # use system mode mask if set + mov %edx, %esi # save store_mask for later mov %eax, %ecx # ecx = new_cpsr