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