1 #include <assert.h>
2
3 #include "armv5te/asmcode.h"
4 #include "armv5te/cpu.h"
5 #include "armv5te/cpudefs.h"
6 #include "armv5te/debug.h"
7 #include "armv5te/mmu.h"
8
9 // Detect overflow after an addition or subtraction
10 #define ADD_OVERFLOW(left, right, sum) ((int32_t)(((left) ^ (sum)) & ((right) ^ (sum))) < 0)
11 #define SUB_OVERFLOW(left, right, sum) ((int32_t)(((left) ^ (right)) & ((left) ^ (sum))) < 0)
12
add(uint32_t left,uint32_t right,int carry,int setcc)13 static uint32_t add(uint32_t left, uint32_t right, int carry, int setcc) {
14 uint32_t sum = left + right + carry;
15 if (!setcc)
16 return sum;
17
18 if (sum < left) carry = 1;
19 if (sum > left) carry = 0;
20 arm.cpsr_c = carry;
21 arm.cpsr_v = ADD_OVERFLOW(left, right, sum);
22 return sum;
23 }
24
25 // "uint8_t shift_val" is correct here. If it is a shift by register, only the bottom 8 bits are looked at.
shift(uint32_t value,uint8_t shift_type,uint8_t shift_val,bool setcc,bool has_rs)26 static uint32_t shift(uint32_t value, uint8_t shift_type, uint8_t shift_val, bool setcc, bool has_rs)
27 {
28 if(shift_val == 0)
29 {
30 if(unlikely(!has_rs))
31 {
32 switch(shift_type)
33 {
34 case SH_ROR:
35 {
36 // RRX
37 bool carry = arm.cpsr_c;
38 if(setcc) arm.cpsr_c = value & 1;
39 return value >> 1 | uint32_t(carry) << 31;
40 }
41 case SH_ASR:
42 case SH_LSR: // #32 is encoded as LSR #0
43 return shift(value, shift_type, 32, setcc, false);
44 }
45 }
46
47 return value;
48 }
49 else if(likely(shift_val < 32))
50 {
51 switch(shift_type)
52 {
53 case SH_LSL:
54 if(setcc) arm.cpsr_c = (value >> (32 - shift_val)) & 1;
55 return value << shift_val;
56 case SH_LSR:
57 if(setcc) arm.cpsr_c = (value >> (shift_val - 1)) & 1;
58 return value >> shift_val;
59 case SH_ASR:
60 if(setcc) arm.cpsr_c = (value >> (shift_val - 1)) & 1;
61 if(value & (1u << 31)) //TODO: Verify!
62 return ~((~value) >> shift_val);
63 else
64 return value >> shift_val;
65 case SH_ROR:
66 if(setcc) arm.cpsr_c = (value >> (shift_val - 1)) & 1;
67 return value >> shift_val | (value << (32 - shift_val));
68 }
69 }
70 else if(shift_val == 32 || shift_type == SH_ASR || shift_type == SH_ROR)
71 {
72 switch(shift_type)
73 {
74 case SH_LSL: if(setcc) arm.cpsr_c = value & 1; return 0;
75 case SH_LSR: if(setcc) arm.cpsr_c = !!(value & (1u << 31)); return 0;
76 case SH_ASR: if(setcc) arm.cpsr_c = !!(value & (1u << 31));
77 if(value & (1u << 31))
78 return 0xFFFFFFFF;
79 else
80 return 0x00000000;
81 case SH_ROR: return shift(value, SH_ROR, shift_val & 0b11111, setcc, false);
82 }
83 }
84 else // shift_val > 32
85 {
86 if(setcc)
87 arm.cpsr_c = 0;
88 return 0;
89 }
90
91 return 0;
92 }
93
addr_mode_2(Instruction i)94 static uint32_t addr_mode_2(Instruction i)
95 {
96 if(!i.mem_proc.not_imm)
97 return i.mem_proc.immed;
98
99 return shift(reg_pc(i.mem_proc.rm), i.mem_proc.shift, i.mem_proc.shift_imm, false, false);
100 }
101
rotated_imm(Instruction i,bool setcc)102 static uint32_t rotated_imm(Instruction i, bool setcc)
103 {
104 uint32_t imm = i.data_proc.immed_8;
105 uint8_t count = i.data_proc.rotate_imm << 1;
106 if(count == 0)
107 return imm;
108
109 imm = (imm >> count) | (imm << (32 - count));
110 if(setcc)
111 arm.cpsr_c = !!(imm & (1u << 31));
112 return imm;
113 }
114
addr_mode_1(Instruction i,bool setcc)115 static uint32_t addr_mode_1(Instruction i, bool setcc)
116 {
117 if(i.data_proc.imm)
118 return rotated_imm(i, setcc);
119
120 if(i.data_proc.reg_shift)
121 return shift(reg_pc(i.data_proc.rm), i.data_proc.shift, reg(i.data_proc.rs), setcc, true);
122 else
123 return shift(reg_pc(i.data_proc.rm), i.data_proc.shift, i.data_proc.shift_imm, setcc, false);
124 }
125
set_nz_flags(uint32_t value)126 static inline void set_nz_flags(uint32_t value) {
127 arm.cpsr_n = value >> 31;
128 arm.cpsr_z = value == 0;
129 }
130
set_nz_flags_64(uint64_t value)131 static inline void set_nz_flags_64(uint64_t value) {
132 arm.cpsr_n = value >> 63;
133 arm.cpsr_z = value == 0;
134 }
135
do_arm_instruction(Instruction i)136 void do_arm_instruction(Instruction i)
137 {
138 bool exec = true;
139
140 // Shortcut for unconditional instructions
141 if(likely(i.cond == CC_AL))
142 goto always;
143
144 switch(i.cond)
145 {
146 case CC_EQ: case CC_NE: exec = arm.cpsr_z; break;
147 case CC_CS: case CC_CC: exec = arm.cpsr_c; break;
148 case CC_MI: case CC_PL: exec = arm.cpsr_n; break;
149 case CC_VS: case CC_VC: exec = arm.cpsr_v; break;
150 case CC_HI: case CC_LS: exec = !arm.cpsr_z && arm.cpsr_c; break;
151 case CC_GE: case CC_LT: exec = arm.cpsr_n == arm.cpsr_v; break;
152 case CC_GT: case CC_LE: exec = !arm.cpsr_z && arm.cpsr_n == arm.cpsr_v; break;
153 case CC_NV:
154 if((i.raw & 0xFD70F000) == 0xF550F000)
155 return;
156 else if((i.raw & 0xFE000000) == 0xFA000000)
157 {
158 // BLX
159 arm.reg[14] = arm.reg[15];
160 arm.reg[15] += 4 + ((int32_t) (i.raw << 8) >> 6) + (i.raw >> 23 & 2);//TODO: this signed int shift is undefined behavior by the C standard
161 arm.cpsr_low28 |= 0x20; // Enter Thumb mode
162 return;
163 }
164 else
165 undefined_instruction();
166
167 return;
168 }
169
170 exec ^= i.cond & 1;
171
172 if(!exec)
173 return;
174
175 always:
176 uint32_t insn = i.raw;
177
178 if((insn & 0xE000090) == 0x0000090)
179 {
180 // MUL, SWP, etc.
181 // LDRH, STRSH, etc.
182 int type = insn >> 5 & 3;
183 if (type == 0) {
184 if ((insn & 0xFC000F0) == 0x0000090) {
185 /* MUL, MLA: 32x32 to 32 multiplications */
186 uint32_t res = reg(insn & 15)
187 * reg(insn >> 8 & 15);
188 if (insn & 0x0200000)
189 res += reg(insn >> 12 & 15);
190
191 set_reg(insn >> 16 & 15, res);
192 if (insn & 0x0100000) set_nz_flags(res);
193 } else if ((insn & 0xF8000F0) == 0x0800090) {
194 /* UMULL, UMLAL, SMULL, SMLAL: 32x32 to 64 multiplications */
195 uint32_t left = reg(insn & 15);
196 uint32_t right = reg(insn >> 8 & 15);
197 uint32_t reg_lo = insn >> 12 & 15;
198 uint32_t reg_hi = insn >> 16 & 15;
199
200 if (reg_lo == reg_hi)
201 error("RdLo and RdHi cannot be same for 64-bit multiply");
202
203 uint64_t res;
204 if (insn & 0x0400000) res = (int64_t)(int32_t)left * (int32_t)right;
205 else res = (uint64_t)left * right;
206 if (insn & 0x0200000) {
207 /* Accumulate */
208 res += (uint64_t)reg(reg_hi) << 32 | reg(reg_lo);
209 }
210
211 set_reg(reg_lo, res);
212 set_reg(reg_hi, res >> 32);
213 if (insn & 0x0100000) set_nz_flags_64(res);
214 } else if ((insn & 0xFB00FF0) == 0x1000090) {
215 /* SWP, SWPB */
216 uint32_t addr = reg(insn >> 16 & 15);
217 uint32_t ld, st = reg(insn & 15);
218 if (insn & 0x0400000) {
219 ld = read_byte(addr); write_byte(addr, st);
220 } else {
221 ld = read_word(addr); write_word(addr, st);
222 }
223 set_reg(insn >> 12 & 15, ld);
224 } else {
225 undefined_instruction();
226 }
227 } else {
228 /* Load/store halfword, signed byte/halfword, or doubleword */
229 int base_reg = insn >> 16 & 15;
230 int data_reg = insn >> 12 & 15;
231 int offset = (insn & (1 << 22))
232 ? (insn & 0x0F) | (insn >> 4 & 0xF0)
233 : reg(insn & 15);
234 bool writeback = false;
235 uint32_t addr = reg_pc(base_reg);
236
237 if (!(insn & (1 << 23))) // Subtracted offset
238 offset = -offset;
239
240 if (insn & (1 << 24)) { // Offset or pre-indexed addressing
241 addr += offset;
242 offset = 0;
243 writeback = insn & (1 << 21);
244 } else {
245 if(insn & (1 << 21))
246 mmu_check_priv(addr, !((insn & (1 << 20)) || type == 2));
247
248 writeback = true;
249 }
250
251 if (insn & (1 << 20)) {
252 uint32_t data;
253 if (base_reg == data_reg && writeback)
254 error("Load instruction modifies base register twice");
255 if (type == 1) data = read_half(addr); /* LDRH */
256 else if (type == 2) data = (int8_t) read_byte(addr); /* LDRSB */
257 else data = (int16_t)read_half(addr); /* LDRSH */
258 set_reg(data_reg, data);
259 } else if (type == 1) { /* STRH */
260 write_half(addr, reg(data_reg));
261 } else {
262 if (data_reg & 1) error("LDRD/STRD with odd-numbered data register");
263 if (type == 2) { /* LDRD */
264 if ((base_reg & ~1) == data_reg && writeback)
265 error("Load instruction modifies base register twice");
266 uint32_t low = read_word(addr);
267 uint32_t high = read_word(addr + 4);
268 set_reg(data_reg, low);
269 set_reg(data_reg + 1, high);
270 } else { /* STRD */
271 write_word(addr, reg(data_reg));
272 write_word(addr + 4, reg(data_reg + 1));
273 }
274 }
275 if (writeback)
276 set_reg(base_reg, addr + offset);
277 }
278 }
279 else if((insn & 0xD900000) == 0x1000000)
280 {
281 // BLX, MRS, MSR, SMUL, etc.
282 if ((insn & 0xFFFFFD0) == 0x12FFF10) {
283 /* B(L)X: Branch(, link,) and exchange T bit */
284 uint32_t target = reg_pc(insn & 15);
285 if (insn & 0x20)
286 arm.reg[14] = arm.reg[15];
287 set_reg_bx(15, target);
288 } else if ((insn & 0xFBF0FFF) == 0x10F0000) {
289 /* MRS: Move reg <- status */
290 set_reg(insn >> 12 & 15, (insn & 0x0400000) ? get_spsr() : get_cpsr());
291 } else if ((insn & 0xFB0FFF0) == 0x120F000 ||
292 (insn & 0xFB0F000) == 0x320F000) {
293 /* MSR: Move status <- reg/imm */
294 uint32_t val, mask = 0;
295 if (insn & 0x2000000)
296 val = rotated_imm(i, false);
297 else
298 val = reg(insn & 15);
299 if (insn & 0x0080000) mask |= 0xFF000000;
300 if (insn & 0x0040000) mask |= 0x00FF0000;
301 if (insn & 0x0020000) mask |= 0x0000FF00;
302 if (insn & 0x0010000) mask |= 0x000000FF;
303 if (insn & 0x0400000)
304 set_spsr(val, mask);
305 else
306 set_cpsr(val, mask);
307 } else if ((insn & 0xF900090) == 0x1000080) {
308 int32_t left = reg(insn & 15);
309 int16_t right = reg((insn >> 8) & 15) >> ((insn & 0x40) ? 16 : 0);
310 int32_t product;
311 int type = insn >> 21 & 3;
312
313 if (type == 1) {
314 /* SMULW<y>, SMLAW<y>: Signed 32x16 to 48 multiply, uses only top 32 bits */
315 product = (int64_t)left * right >> 16;
316 if (!(insn & 0x20))
317 goto accumulate;
318 } else {
319 /* SMUL<x><y>, SMLA<x><y>, SMLAL<x><y>: Signed 16x16 to 32 multiply */
320 product = (int16_t)(left >> ((insn & 0x20) ? 16 : 0)) * right;
321 }
322 if (type == 2) {
323 /* SMLAL<x><y>: 64-bit accumulate */
324 uint32_t reg_lo = insn >> 12 & 15;
325 uint32_t reg_hi = insn >> 16 & 15;
326 int64_t sum;
327 if (reg_lo == reg_hi)
328 error("RdLo and RdHi cannot be same for 64-bit accumulate");
329 sum = product + ((uint64_t)reg(reg_hi) << 32 | reg(reg_lo));
330 set_reg(reg_lo, sum);
331 set_reg(reg_hi, sum >> 32);
332 } else if (type == 0) accumulate: {
333 /* SMLA<x><y>, SMLAW<y>: 32-bit accumulate */
334 int32_t acc = reg(insn >> 12 & 15);
335 int32_t sum = product + acc;
336 /* Set Q flag on overflow */
337 arm.cpsr_low28 |= ADD_OVERFLOW(product, acc, sum) << 27;
338 set_reg(insn >> 16 & 15, sum);
339 } else {
340 /* SMUL<x><y>, SMULW<y>: No accumulate */
341 set_reg(insn >> 16 & 15, product);
342 }
343 } else if ((insn & 0xF900FF0) == 0x1000050) {
344 /* QADD, QSUB, QDADD, QDSUB: Saturated arithmetic */
345 int32_t left = reg(insn & 15);
346 int32_t right = reg(insn >> 16 & 15);
347 int32_t res, overflow;
348 if (insn & 0x400000) {
349 /* Doubled right operand */
350 res = right << 1;
351 if (ADD_OVERFLOW(right, right, res)) {
352 /* Overflow, set Q flag and saturate */
353 arm.cpsr_low28 |= 1 << 27;
354 res = (res < 0) ? 0x7FFFFFFF : 0x80000000;
355 }
356 right = res;
357 }
358 if (!(insn & 0x200000)) {
359 res = left + right;
360 overflow = ADD_OVERFLOW(left, right, res);
361 } else {
362 res = left - right;
363 overflow = SUB_OVERFLOW(left, right, res);
364 }
365 if (overflow) {
366 /* Set Q flag and saturate */
367 arm.cpsr_low28 |= 1 << 27;
368 res = (res < 0) ? 0x7FFFFFFF : 0x80000000;
369 }
370 set_reg(insn >> 12 & 15, res);
371 } else if ((insn & 0xFFF0FF0) == 0x16F0F10) {
372 /* CLZ: Count leading zeros */
373 int32_t value = reg(insn & 15);
374 uint32_t zeros;
375 for (zeros = 0; zeros < 32 && value >= 0; zeros++)
376 value <<= 1;
377 set_reg(insn >> 12 & 15, zeros);
378 } else if ((insn & 0xFFF000F0) == 0xE1200070) {
379 gui_debug_printf("Software breakpoint at %08X (%04X)\n",
380 arm.reg[15], (insn >> 4 & 0xFFF0) | (insn & 0xF));
381 debugger(DBG_EXEC_BREAKPOINT, 0);
382 } else
383 undefined_instruction();
384 }
385 else if(likely((insn & 0xC000000) == 0x0000000))
386 {
387 // Data processing
388 bool carry = arm.cpsr_c,
389 setcc = i.data_proc.s;
390
391 uint32_t left = reg_pc(i.data_proc.rn),
392 right = addr_mode_1(i, setcc),
393 res = 0;
394
395 switch(i.data_proc.op)
396 {
397 case OP_AND: res = left & right; break;
398 case OP_EOR: res = left ^ right; break;
399 case OP_SUB: res = add( left, ~right, 1, setcc); break;
400 case OP_RSB: res = add(~left, right, 1, setcc); break;
401 case OP_ADD: res = add( left, right, 0, setcc); break;
402 case OP_ADC: res = add( left, right, carry, setcc); break;
403 case OP_SBC: res = add( left, ~right, carry, setcc); break;
404 case OP_RSC: res = add(~left, right, carry, setcc); break;
405 case OP_TST: res = left & right; break;
406 case OP_TEQ: res = left ^ right; break;
407 case OP_CMP: res = add( left, ~right, 1, setcc); break;
408 case OP_CMN: res = add( left, right, 0, setcc); break;
409 case OP_ORR: res = left | right; break;
410 case OP_MOV: res = right; break;
411 case OP_BIC: res = left & ~right; break;
412 case OP_MVN: res = ~right; break;
413 }
414
415 if(i.data_proc.op < OP_TST || i.data_proc.op > OP_CMN)
416 set_reg_pc(i.data_proc.rd, res);
417
418 if(setcc)
419 {
420 // Used for returning from exceptions, for instance
421 if(i.data_proc.rd == 15)
422 set_cpsr_full(get_spsr());
423 else
424 {
425 arm.cpsr_n = res >> 31;
426 arm.cpsr_z = res == 0;
427 }
428 }
429 }
430 else if((insn & 0xFF000F0) == 0x7F000F0)
431 undefined_instruction();
432 else if((insn & 0xC000000) == 0x4000000)
433 {
434 // LDR, STRB, etc.
435 uint32_t base = reg_pc(i.mem_proc.rn),
436 offset = addr_mode_2(i);
437 if(!i.mem_proc.u)
438 offset = -offset;
439
440 // Pre-indexed or offset
441 if(i.mem_proc.p)
442 base += offset; // Writeback for pre-indexed handled after access
443 else if(i.mem_proc.w) // Usermode Access
444 mmu_check_priv(base, !i.mem_proc.l);
445
446 // Byte access
447 if(i.mem_proc.b)
448 {
449 if(i.mem_proc.l) set_reg_bx(i.mem_proc.rd, read_byte(base));
450 else write_byte(base, reg_pc_mem(i.mem_proc.rd));
451 }
452 else
453 {
454 if(i.mem_proc.l) set_reg_bx(i.mem_proc.rd, read_word(base));
455 else write_word(base, reg_pc_mem(i.mem_proc.rd));
456 }
457
458 // Post-indexed addressing
459 if(!i.mem_proc.p)
460 base += offset;
461
462 // Writeback
463 if(!i.mem_proc.p || i.mem_proc.w)
464 set_reg(i.mem_proc.rn, base);
465 }
466 else if((insn & 0xE000000) == 0x8000000)
467 {
468 // LDM, STM, etc.
469 int base_reg = insn >> 16 & 15;
470 uint32_t addr = reg(base_reg);
471 uint32_t new_base = addr;
472 int count = __builtin_popcount(i.mem_multi.reglist);
473
474 if (i.mem_multi.u) { // Increasing
475 if (i.mem_multi.w) // Writeback
476 new_base += count * 4;
477 if (i.mem_multi.p) // Preincrement
478 addr += 4;
479 } else { // Decreasing
480 addr -= count * 4;
481 if (i.mem_multi.w) // Writeback
482 new_base = addr;
483 if (!i.mem_multi.p) // Postdecrement
484 addr += 4;
485 }
486
487 for (unsigned reg = 0, reglist = i.mem_multi.reglist; reglist && reg < 15; reglist >>= 1, reg++) {
488 if ((reglist & 1) == 0)
489 continue;
490
491 uint32_t *reg_ptr = &arm.reg[reg];
492 if (i.mem_multi.s && !i.mem_multi.w && !(i.mem_multi.reglist & (1<<15))) {
493 // User-mode registers
494 int mode = arm.cpsr_low28 & 0x1F;
495 if (reg >= 13) {
496 if (mode != MODE_USR && mode != MODE_SYS) reg_ptr = &arm.r13_usr[reg - 13];
497 } else if (reg >= 8) {
498 if (mode == MODE_FIQ) reg_ptr = &arm.r8_usr[reg - 8];
499 }
500 }
501 if (i.mem_multi.l) { // Load
502 if (reg_ptr == &arm.reg[base_reg]) {
503 if (i.mem_multi.w) // Writeback
504 error("Load instruction modifies base register twice");
505 reg_ptr = &new_base;
506 }
507 *reg_ptr = read_word(addr);
508 } else { // Store
509 write_word(addr, *reg_ptr);
510 }
511 addr += 4;
512 }
513 if (i.mem_multi.reglist & (1 << 15)) {
514 if (i.mem_multi.l) // Load
515 set_reg_bx(15, read_word(addr));
516 else // Store
517 write_word(addr, reg_pc_mem(15));
518 }
519 arm.reg[base_reg] = new_base;
520 if (i.mem_multi.l && i.mem_multi.s && i.mem_multi.reglist & (1<<15))
521 set_cpsr_full(get_spsr());
522 }
523 else if((insn & 0xE000000) == 0xA000000)
524 {
525 // B and BL
526 if(i.branch.l)
527 arm.reg[14] = arm.reg[15];
528 arm.reg[15] += (int32_t) (i.branch.immed << 8) >> 6;//TODO: this signed int shift is undefined behavior by the C standard
529 arm.reg[15] += 4;
530 if(arm.reg[15] >= 0x2009B130 && arm.reg[15] <= 0x200C7EEB && i.branch.l)
531 gui_debug_printf("ARM DAL function call, jump from 0x%08X to 0x%08X\n", arm.reg[14] - 4, arm.reg[15]);
532 }
533 else if((insn & 0xF000F10) == 0xE000F10)
534 do_cp15_instruction(i);
535 else if((insn & 0xF000F10) == 0xE000E10)
536 do_cp14_instruction(i);
537 else if((insn & 0xF000000) == 0xF000000)
538 cpu_exception(EX_SWI);
539 else
540 undefined_instruction();
541 }
542