Skip to content

Commit 27238de

Browse files
Ace314159Ace314159
committed
Fix instruction decoding and implement data processing
1 parent a61620b commit 27238de

File tree

7 files changed

+484
-64
lines changed

7 files changed

+484
-64
lines changed

‎.gitignore

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,2 @@
1-
/target
1+
target
22
ROMs

‎core/Cargo.lock

+14
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎core/src/cpu/mod.rs

+176-40
Original file line numberDiff line numberDiff line change
@@ -1,51 +1,97 @@
11
mod registers;
22

3-
use crate::mmu::MMU;
3+
use crate::mmu::IMMU;
44
use crate::mmu::Cycle;
55
use registers::RegValues;
66
use registers::Reg;
77

8+
#[cfg(test)]
9+
mod tests;
10+
811

912
pub struct CPU {
1013
regs: RegValues,
14+
instr_buffer: [u32; 2],
15+
p: bool,
1116
}
1217

1318
impl CPU {
14-
pub fn new() -> CPU {
15-
CPU {
19+
pub fn new<M>(mmu: &M) -> CPU where M: IMMU {
20+
let mut cpu = CPU {
1621
regs: RegValues::new(),
22+
instr_buffer: [0; 2],
23+
p: true,
24+
};
25+
cpu.fill_instr_buffer(mmu);
26+
cpu
27+
}
28+
29+
fn fill_instr_buffer<M>(&mut self, mmu: &M) where M: IMMU {
30+
if self.regs.get_t() {
31+
unimplemented!("Thumb instruction set not implemented!");
32+
} else {
33+
self.instr_buffer[0] = mmu.read32(self.regs.pc & !0x3);
34+
self.regs.pc = self.regs.pc.wrapping_add(4);
35+
self.instr_buffer[1] = mmu.read32(self.regs.pc & !0x3);
1736
}
1837
}
1938

20-
pub fn emulate_instr(&mut self, mmu: &mut MMU) {
39+
pub fn emulate_instr<M>(&mut self, mmu: &mut M) where M: IMMU {
2140
if self.regs.get_t() { self.emulate_thumb_instr(mmu) }
2241
else { self.emulate_arm_instr(mmu) }
2342
}
2443

25-
pub fn emulate_thumb_instr(&mut self, mmu: &mut MMU) {
44+
pub fn emulate_thumb_instr<M>(&mut self, mmu: &mut M) where M: IMMU {
2645
unimplemented!("Thumb instruction set not implemented!")
2746
}
2847

29-
pub fn emulate_arm_instr(&mut self, mmu: &mut MMU) {
30-
let pc = self.regs.get_pc() & !0x3; // Align pc
31-
self.regs.set_pc(pc.wrapping_add(4));
32-
let instr = mmu.read32(pc);
48+
pub fn emulate_arm_instr<M>(&mut self, mmu: &mut M) where M: IMMU {
49+
let pc = self.regs.pc & !0x3;
50+
if self.p {
51+
use Reg::*;
52+
println!("{:08x} r0:{:08x} r1:{:08x} r2:{:08x} r3:{:08x} r4:{:08x} r5:{:08x} r6:{:08x} \
53+
r7:{:08x} r8:{:08x} r9:{:08x} r10:{:08x} r11:{:08x} r12:{:08x} sp:{:08x} lr:{:08x}",
54+
pc.wrapping_sub(8), self.regs.get_reg(R0), self.regs.get_reg(R1), self.regs.get_reg(R2),
55+
self.regs.get_reg(R3), self.regs.get_reg(R4), self.regs.get_reg(R5), self.regs.get_reg(R6),
56+
self.regs.get_reg(R7), self.regs.get_reg(R8), self.regs.get_reg(R9), self.regs.get_reg(R10),
57+
self.regs.get_reg(R11), self.regs.get_reg(R12), self.regs.get_reg(R13), self.regs.get_reg(R14));
58+
}
59+
let instr = self.instr_buffer[0];
60+
self.instr_buffer[0] = self.instr_buffer[1];
61+
self.regs.pc = self.regs.pc.wrapping_add(4);
62+
self.instr_buffer[1] = mmu.read32(pc);
3363

3464
if self.should_exec(instr) {
35-
if (instr >> 4) & 0xFFFFFF == 0b0001_0010_1111_1111_1111 { self.branch_and_exchange(instr) }
36-
else if (instr >> 25) & 0x7 == 0b101 { self.branch_branch_with_link(instr) }
37-
else if (instr >> 26) & 0x3 == 0b00 { self.data_proc_psr_transfer(instr) }
38-
else if (instr >> 22) & 0x3F == 0b00_0000 { self.mul(instr) }
39-
else if (instr >> 23) & 0x1F == 0b0_0001 { self.mul_long(instr) }
40-
else if (instr >> 26) & 0x3 == 0b01 { self.single_data_transfer(instr) }
41-
else if (instr >> 25) & 0x7 == 0b000 { self.halfword_and_signed_data_transfer(instr) }
42-
else if (instr >> 25) & 0x7 == 0b100 { self.block_data_transfer(instr) }
43-
else if (instr >> 23) & 0x1F == 0b0_0010 { self.single_data_swap(instr) }
44-
else if (instr >> 24) & 0xF == 0b1111 { self.software_interrupt(instr) }
45-
else if (instr >> 24) & 0xF == 0b1110 { self.coprocessor(instr) }
46-
else if (instr >> 25) & 0x7 == 0b110 { self.coprocessor(instr) }
47-
else if (instr >> 25) & 0x7 == 0b011 { self.undefined_instr(instr) }
48-
else { panic!("Unexpected instruction") }
65+
if instr & 0b1111_1111_1111_1111_1111_1111_0000 == 0b0001_0010_1111_1111_1111_0001_0000 {
66+
self.branch_and_exchange(mmu);
67+
} else if instr & 0b1111_1100_0000_0000_0000_1111_0000 == 0b0000_0000_0000_0000_0000_1001_0000 {
68+
self.mul(instr, mmu);
69+
} else if instr & 0b1111_1000_0000_0000_0000_1111_0000 == 0b0000_1000_0000_0000_0000_1001_0000 {
70+
self.mul_long(instr, mmu);
71+
} else if instr & 0b1111_1000_0000_0000_1111_1111_0000 == 0b0001_0000_0000_0000_0000_1001_0000 {
72+
self.single_data_swap(instr, mmu);
73+
} else if instr & 0b1110_0000_0000_0000_0000_1001_0000 == 0b0000_0000_0000_0000_0000_1001_0000 {
74+
self.halfword_and_signed_data_transfer(instr, mmu);
75+
} else if instr & 0b1101_1001_0000_0000_0000_0000_0000 == 0b0001_0000_0000_0000_0000_0000_0000 {
76+
self.psr_transfer(instr, mmu);
77+
} else if instr & 0b1100_0000_0000_0000_0000_0000_0000 == 0b0000_0000_0000_0000_0000_0000_0000 {
78+
self.data_proc(instr, mmu);
79+
} else if instr & 0b1100_0000_0000_0000_0000_0000_0000 == 0b0100_0000_0000_0000_0000_0000_0000 {
80+
self.single_data_transfer(instr, mmu);
81+
} else if instr & 0b1110_0000_0000_0000_0000_0000_0000 == 0b1000_0000_0000_0000_0000_0000_0000 {
82+
self.block_data_transfer(instr, mmu);
83+
} else if instr & 0b1110_0000_0000_0000_0000_0000_0000 == 0b1010_0000_0000_0000_0000_0000_0000 {
84+
self.branch_branch_with_link(instr, mmu);
85+
} else if instr & 0b1111_0000_0000_0000_0000_0000_0000 == 0b1111_0000_0000_0000_0000_0000_0000 {
86+
self.software_interrupt(instr, mmu);
87+
} else if instr & 0b1110_0000_0000_0000_0000_0000_0000 == 0b1100_0000_0000_0000_0000_0000_0000 {
88+
self.coprocessor(instr, mmu);
89+
} else if instr & 0b1111_0000_0000_0000_0000_0000_0000 == 0b1110_0000_0000_0000_0000_0000_0000 {
90+
self.coprocessor(instr, mmu);
91+
} else {
92+
assert_eq!(instr & 0b1110_0000_0000_0000_0000_0001_0000, 0b1110_0000_0000_0000_0000_0001_0000);
93+
self.undefined_instr(instr, mmu);
94+
}
4995
} else {
5096
mmu.inc_clock(1, Cycle::S, pc);
5197
}
@@ -74,71 +120,161 @@ impl CPU {
74120
}
75121

76122
// ARM.3: Branch and Exchange (BX)
77-
fn branch_and_exchange(&mut self, instr: u32) {
123+
fn branch_and_exchange<M>(&mut self, mmu: &mut M) where M: IMMU {
78124
unimplemented!("ARM.3: Branch and Exchange (BX) not implemented!");
79125
}
80126

81127
// ARM.4: Branch and Branch with Link (B, BL)
82-
fn branch_branch_with_link(&mut self, instr: u32) {
128+
fn branch_branch_with_link<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
83129
let opcode = (instr >> 24) & 0x1;
84130
let offset = instr & 0xFF_FFFF;
85131
let offset = if (offset >> 23) == 1 { 0xFF00_0000 | offset } else { offset };
86-
let pc = self.regs.get_pc();
87132

88-
if opcode == 1 { self.regs.set_reg(Reg::R14, pc) } // Branch with Link
89-
self.regs.set_pc(pc.wrapping_add(offset * 4));
133+
if opcode == 1 { self.regs.set_reg(Reg::R14, self.regs.pc.wrapping_sub(4)) } // Branch with Link
134+
mmu.inc_clock(2, Cycle::S, self.regs.pc);
135+
self.regs.pc = self.regs.pc.wrapping_add(offset << 2);
136+
self.fill_instr_buffer(mmu);
137+
mmu.inc_clock(1, Cycle::N, self.regs.pc);
90138
}
91139

92140
// ARM.5: Data Processing
93-
// ARM.6: PSR Transfer (MRS, MSR)
94-
fn data_proc_psr_transfer(&mut self, instr: u32) {
95-
unimplemented!("ARM.5: Data Processing and ARM.6: PSR Transfer (MRS, MSR) not implemented!");
141+
fn data_proc<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
142+
let mut change_status = (instr >> 20) & 0x1 != 0;
143+
let immediate_op2 = (instr >> 25) & 0x1 != 0;
144+
let op2 = if immediate_op2 {
145+
let shift = (instr >> 8) & 0xF;
146+
(instr & 0xFF).rotate_right(shift * 2)
147+
} else {
148+
let shift_by_reg = (instr >> 4) & 0x1 != 0;
149+
let shift = if shift_by_reg {
150+
assert_eq!((instr >> 7) & 0x1, 0);
151+
let shift = self.regs.get_reg_i((instr >> 8) & 0xF) & 0xFF;
152+
if shift == 0 { change_status = false }
153+
shift
154+
} else {
155+
(instr >> 7) & 0x1F
156+
};
157+
let shift_type = (instr >> 5) & 0x3;
158+
let op2 = self.regs.get_reg_i(instr & 0xF);
159+
if !shift_by_reg && shift == 0 {
160+
match shift_type {
161+
0 => op2,
162+
1 => {
163+
if change_status { self.regs.set_c(op2 >> 31 != 0) }
164+
0
165+
},
166+
2 => {
167+
let bit = op2 >> 31 != 0;
168+
if change_status { self.regs.set_c(bit); }
169+
if bit { 0xFFFF_FFFF } else { 0 } },
170+
3 => {
171+
let new_c = op2 & 0x1 != 0;
172+
let op2 = (self.regs.get_c() as u32) << 31 | op2 >> 1;
173+
if change_status { self.regs.set_c(new_c) }
174+
op2
175+
},
176+
_ => panic!("Invalid Shift type!"),
177+
}
178+
} else {
179+
match shift_type {
180+
0 => { if change_status { self.regs.set_c(op2 << (shift - 1) & 0x8000_0000 != 0); } op2 << shift },
181+
1 => { if change_status { self.regs.set_c(op2 >> (shift - 1) & 0x1 != 0); } op2 >> shift },
182+
2 => { if change_status { self.regs.set_c((op2 as i32) >> (shift - 1) & 0x1 != 0) };
183+
((op2 as i32) >> shift) as u32 },
184+
3 => { if change_status { self.regs.set_c(op2 >> (shift - 1) & 0x1 != 0); } op2.rotate_right(shift) },
185+
_ => panic!("Invalid Shift type!"),
186+
}
187+
}
188+
};
189+
let opcode = (instr >> 21) & 0xF;
190+
let op1 = self.regs.get_reg_i((instr >> 16) & 0xF);
191+
macro_rules! arithmetic { ($op1:expr, $op2:expr, $func:ident, $sub:expr, $add_c:expr) => { {
192+
let result = ($op1 as i32).$func($op2 as i32);
193+
let result2 = if $add_c { result.0.overflowing_add(self.regs.get_c() as i32) }
194+
else { (result.0, false) };
195+
if change_status {
196+
self.regs.set_v(result.1 || result2.1);
197+
let c = $op1.$func($op2).1;
198+
let c = if $sub { !c } else { c };
199+
self.regs.set_c(c);
200+
}
201+
result2.0 as u32
202+
} } }
203+
let result = match opcode {
204+
0x0 | 0x8 => op1 & op2, // AND and TST
205+
0x1 | 0x9 => op1 ^ op2, // EOR and TEQ
206+
0x2 | 0xA => arithmetic!(op1, op2, overflowing_sub, true, false), // SUB and CMP
207+
0x3 => arithmetic!(op2, op1, overflowing_sub, false, false), // RSB
208+
0x4 | 0xB => arithmetic!(op1, op2, overflowing_add, false, false), // ADD and CMN
209+
0x5 => arithmetic!(op1, op2, overflowing_add, false, true), // ADC
210+
0x6 => arithmetic!(op1, !op2, overflowing_add, true, true), // SBC
211+
0x7 => arithmetic!(op2, !op1, overflowing_add, true, true), // RSC
212+
0xC => op1 | op2, // ORR
213+
0xD => op2, // MOV
214+
0xE => op1 & !op2, // BIC
215+
0xF => !op2, // MVN
216+
_ => panic!("Invalid opcode!"),
217+
};
218+
if change_status {
219+
self.regs.set_z(result == 0);
220+
self.regs.set_n(result & 0x8000_0000 != 0);
221+
} else { assert_eq!(opcode & 0xC != 0x8, true) }
222+
if opcode & 0xC != 0x8 {
223+
let reg = (instr >> 12) & 0xF;
224+
self.regs.set_reg_i(reg, result);
225+
if reg == 15 { self.fill_instr_buffer(mmu); }
226+
}
96227
}
97228

229+
// ARM.6: PSR Transfer (MRS, MSR)
230+
fn psr_transfer<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
231+
unimplemented!("// ARM.6: PSR Transfer (MRS, MSR) not implemented!");
232+
}
233+
98234
// ARM.7: Multiply and Multiply-Accumulate (MUL, MLA)
99-
fn mul(&mut self, instr: u32) {
235+
fn mul<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
100236
unimplemented!("ARM.7: Multiply and Multiply-Accumulate (MUL, MLA) not implemented!");
101237
}
102238

103239
// ARM.8: Multiply Long and Multiply-Accumulate Long (MULL, MLAL)
104-
fn mul_long(&mut self, instr: u32) {
240+
fn mul_long<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
105241
unimplemented!("ARM.8: Multiply Long and Multiply-Accumulate Long (MULL, MLAL) not implemented!");
106242
}
107243

108244
// ARM.9: Single Data Transfer (LDR, STR)
109-
fn single_data_transfer(&mut self, instr: u32) {
245+
fn single_data_transfer<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
110246
unimplemented!("ARM.9: Single Data Transfer (LDR, STR) not implemented!");
111247
}
112248

113249
// ARM.10: Halfword and Signed Data Transfer (STRH,LDRH,LDRSB,LDRSH)
114-
fn halfword_and_signed_data_transfer(&mut self, instr: u32) {
250+
fn halfword_and_signed_data_transfer<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
115251
unimplemented!("ARM.10: Halfword and Signed Data Transfer (STRH,LDRH,LDRSB,LDRSH) not implemented!");
116252
}
117253

118254
// ARM.11: Block Data Transfer (LDM,STM)
119-
fn block_data_transfer(&mut self, instr: u32) {
255+
fn block_data_transfer<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
120256
unimplemented!("ARM.11: Block Data Transfer (LDM,STM) not implemented!");
121257
}
122258

123259
// ARM.12: Single Data Swap (SWP)
124-
fn single_data_swap(&mut self, instr: u32) {
260+
fn single_data_swap<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
125261
unimplemented!("ARM.12: Single Data Swap (SWP) not implemented!");
126262
}
127263

128264
// ARM.13: Software Interrupt (SWI)
129-
fn software_interrupt(&mut self, instr: u32) {
265+
fn software_interrupt<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
130266
unimplemented!("ARM.13: Software Interrupt (SWI) not implemented!");
131267
}
132268

133269
// ARM.14: Coprocessor Data Operations (CDP)
134270
// ARM.15: Coprocessor Data Transfers (LDC,STC)
135271
// ARM.16: Coprocessor Register Transfers (MRC, MCR)
136-
fn coprocessor(&mut self, instr: u32) {
272+
fn coprocessor<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
137273
unimplemented!("Coprocessor not implemented!");
138274
}
139275

140276
// ARM.17: Undefined Instruction
141-
fn undefined_instr(&mut self, instr: u32) {
277+
fn undefined_instr<M>(&mut self, instr: u32, mmu: &mut M) where M: IMMU {
142278
unimplemented!("ARM.17: Undefined Instruction not implemented!");
143279
}
144280
}

0 commit comments

Comments
 (0)