1177 lines
36 KiB
C
1177 lines
36 KiB
C
//
|
|
// ARM7 processor emulator - THUMB support
|
|
// version 2.0 / 2008-02-08
|
|
// By R. Belmont and SGINut
|
|
//
|
|
|
|
#include "arm7.h"
|
|
#include "arm7i.h"
|
|
#include "arm7thumb.h"
|
|
|
|
// base cycle counts for Thumb instructions
|
|
static const int thumbCycles[256] =
|
|
{
|
|
// 0 1 2 3 4 5 6 7 8 9 a b c d e f
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3
|
|
1, 1, 1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 4
|
|
2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 5
|
|
2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, // 6
|
|
2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, // 7
|
|
2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, // 8
|
|
2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, // 9
|
|
3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // a
|
|
1, 1, 1, 1, 2, 2, 1, 1, 1, 1, 1, 1, 2, 4, 1, 1, // b
|
|
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // c
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3, // d
|
|
3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // e
|
|
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 // f
|
|
};
|
|
|
|
// utility macros
|
|
|
|
#define IsNeg(i) ((i) >> 31)
|
|
#define IsPos(i) ((~(i)) >> 31)
|
|
|
|
#define N_BIT 31
|
|
#define Z_BIT 30
|
|
#define C_BIT 29
|
|
#define V_BIT 28
|
|
#define SIGN_BIT ((UINT32)(1<<31))
|
|
|
|
#define HandleThumbALUAddFlags(rd, rn, op2) \
|
|
ARM7_SetCPSR( \
|
|
((GET_CPSR &~ (ARM7_CPSR_N | ARM7_CPSR_Z | ARM7_CPSR_V | ARM7_CPSR_C)) \
|
|
| (((!THUMB_SIGN_BITS_DIFFER(rn, op2)) && THUMB_SIGN_BITS_DIFFER(rn, rd)) \
|
|
<< V_BIT) \
|
|
| (((~(rn)) < (op2)) << C_BIT) \
|
|
| HandleALUNZFlags(rd))); \
|
|
R15 += 2;
|
|
|
|
#define HandleThumbALUSubFlags(rd, rn, op2) \
|
|
ARM7_SetCPSR( \
|
|
((GET_CPSR &~ (ARM7_CPSR_N | ARM7_CPSR_Z | ARM7_CPSR_V | ARM7_CPSR_C)) \
|
|
| ((THUMB_SIGN_BITS_DIFFER(rn, op2) && THUMB_SIGN_BITS_DIFFER(rn, rd)) \
|
|
<< V_BIT) \
|
|
| (((IsNeg(rn) & IsPos(op2)) | (IsNeg(rn) & IsPos(rd)) | (IsPos(op2) & IsPos(rd))) ? ARM7_CPSR_C : 0) \
|
|
| HandleALUNZFlags(rd))); \
|
|
R15 += 2;
|
|
|
|
#define HandleALUNZFlags(rd) \
|
|
(((rd) & SIGN_BIT) | ((!(rd)) << Z_BIT))
|
|
|
|
// memory accessors
|
|
#include "arm7memil.c"
|
|
|
|
// public functions
|
|
int ARM7i_Thumb_Step()
|
|
{
|
|
UINT32 readword;
|
|
UINT32 addr, insn;
|
|
UINT32 rm, rn, rs, rd, op2, imm, rrs, rrd;
|
|
INT32 offs;
|
|
UINT32 pc = ARM7.Rx[ARM7_PC];
|
|
int cycles;
|
|
|
|
insn = arm7_read_16(pc & (~1));
|
|
|
|
cycles = (3 - thumbCycles[insn >> 8]);
|
|
|
|
switch( ( insn & THUMB_INSN_TYPE ) >> THUMB_INSN_TYPE_SHIFT )
|
|
{
|
|
case 0x0: /* Logical shifting */
|
|
ARM7_SetCPSR(GET_CPSR &~ (ARM7_CPSR_N | ARM7_CPSR_Z));
|
|
if( insn & THUMB_SHIFT_R ) /* Shift right */
|
|
{
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrs = GET_REGISTER(rs);
|
|
offs = ( insn & THUMB_SHIFT_AMT ) >> THUMB_SHIFT_AMT_SHIFT;
|
|
if( offs != 0 )
|
|
{
|
|
SET_REGISTER( rd, rrs >> offs );
|
|
if( rrs & ( 1 << (offs-1) ) )
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR | ARM7_CPSR_C);
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_C);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SET_REGISTER( rd, 0 );
|
|
if( rrs & 0x80000000 )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
ARM7_SetCPSR(GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
}
|
|
else /* Shift left */
|
|
{
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrs = GET_REGISTER(rs);
|
|
offs = ( insn & THUMB_SHIFT_AMT ) >> THUMB_SHIFT_AMT_SHIFT;
|
|
if( offs != 0 )
|
|
{
|
|
SET_REGISTER( rd, rrs << offs );
|
|
if( rrs & ( 1 << ( 31 - ( offs - 1 ) ) ) )
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR | ARM7_CPSR_C);
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_C);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SET_REGISTER( rd, rrs );
|
|
}
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0x1: /* Arithmetic */
|
|
if( insn & THUMB_INSN_ADDSUB )
|
|
{
|
|
switch( ( insn & THUMB_ADDSUB_TYPE ) >> THUMB_ADDSUB_TYPE_SHIFT )
|
|
{
|
|
case 0x0: /* ADD Rd, Rs, Rn */
|
|
rn = GET_REGISTER( ( insn & THUMB_ADDSUB_RNIMM ) >> THUMB_ADDSUB_RNIMM_SHIFT );
|
|
rs = GET_REGISTER( ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT );
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, rs + rn );
|
|
HandleThumbALUAddFlags( GET_REGISTER(rd), rs, rn );
|
|
break;
|
|
case 0x1: /* SUB Rd, Rs, Rn */
|
|
rn = GET_REGISTER( ( insn & THUMB_ADDSUB_RNIMM ) >> THUMB_ADDSUB_RNIMM_SHIFT );
|
|
rs = GET_REGISTER( ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT );
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, rs - rn );
|
|
HandleThumbALUSubFlags( GET_REGISTER(rd), rs, rn );
|
|
break;
|
|
case 0x2: /* ADD Rd, Rs, #imm */
|
|
imm = ( insn & THUMB_ADDSUB_RNIMM ) >> THUMB_ADDSUB_RNIMM_SHIFT;
|
|
rs = GET_REGISTER( ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT );
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, rs + imm );
|
|
HandleThumbALUAddFlags( GET_REGISTER(rd), rs, imm );
|
|
break;
|
|
case 0x3: /* SUB Rd, Rs, #imm */
|
|
imm = ( insn & THUMB_ADDSUB_RNIMM ) >> THUMB_ADDSUB_RNIMM_SHIFT;
|
|
rs = GET_REGISTER( ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT );
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, rs - imm );
|
|
HandleThumbALUSubFlags( GET_REGISTER(rd), rs,imm );
|
|
break;
|
|
default:
|
|
printf("%08x: G1 Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* ASR.. */
|
|
//if( insn & THUMB_SHIFT_R ) /* Shift right */
|
|
{
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrs = GET_REGISTER(rs);
|
|
offs = ( insn & THUMB_SHIFT_AMT ) >> THUMB_SHIFT_AMT_SHIFT;
|
|
if( offs == 0 )
|
|
{
|
|
offs = 32;
|
|
}
|
|
|
|
if( offs >= 32 )
|
|
{
|
|
if( rrs >> 31 )
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR | ARM7_CPSR_C);
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_C);
|
|
}
|
|
SET_REGISTER( rd, ( rrs & 0x80000000 ) ? 0xFFFFFFFF : 0x00000000 );
|
|
}
|
|
else
|
|
{
|
|
if( ( rrs >> ( offs - 1 ) ) & 1 )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
SET_REGISTER( rd, ( rrs & 0x80000000 ) ? ( ( 0xFFFFFFFF << ( 32 - offs ) ) | ( rrs >> offs ) ) : ( rrs >> offs ) );
|
|
}
|
|
ARM7_SetCPSR(GET_CPSR &~ (ARM7_CPSR_N | ARM7_CPSR_Z));
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
}
|
|
|
|
}
|
|
break;
|
|
case 0x2: /* CMP / MOV */
|
|
if( insn & THUMB_INSN_CMP )
|
|
{
|
|
rn = GET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT );
|
|
op2 = ( insn & THUMB_INSN_IMM );
|
|
rd = rn - op2;
|
|
HandleThumbALUSubFlags( rd, rn, op2 );
|
|
//mame_printf_debug("%08x: xxx Thumb instruction: CMP R%d (%08x), %02x (N=%d, Z=%d, C=%d, V=%d)\n", pc, ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT, GET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT ), op2, N_IS_SET(GET_CPSR) ? 1 : 0, Z_IS_SET(GET_CPSR) ? 1 : 0, C_IS_SET(GET_CPSR) ? 1 : 0, V_IS_SET(GET_CPSR) ? 1 : 0);
|
|
}
|
|
else
|
|
{
|
|
rd = ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT;
|
|
op2 = ( insn & THUMB_INSN_IMM );
|
|
SET_REGISTER( rd, op2 );
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0x3: /* ADD/SUB immediate */
|
|
if( insn & THUMB_INSN_SUB ) /* SUB Rd, #Offset8 */
|
|
{
|
|
rn = GET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT );
|
|
op2 = ( insn & THUMB_INSN_IMM );
|
|
//mame_printf_debug("%08x: Thumb instruction: SUB R%d, %02x\n", pc, ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT, op2);
|
|
rd = rn - op2;
|
|
SET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT, rd );
|
|
HandleThumbALUSubFlags( rd, rn, op2 );
|
|
}
|
|
else /* ADD Rd, #Offset8 */
|
|
{
|
|
rn = GET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT );
|
|
op2 = insn & THUMB_INSN_IMM;
|
|
rd = rn + op2;
|
|
//mame_printf_debug("%08x: Thumb instruction: ADD R%d, %02x\n", pc, ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT, op2);
|
|
SET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT, rd );
|
|
HandleThumbALUAddFlags( rd, rn, op2 );
|
|
}
|
|
break;
|
|
case 0x4: /* Rd & Rm instructions */
|
|
switch( ( insn & THUMB_GROUP4_TYPE ) >> THUMB_GROUP4_TYPE_SHIFT )
|
|
{
|
|
case 0x0:
|
|
switch( ( insn & THUMB_ALUOP_TYPE ) >> THUMB_ALUOP_TYPE_SHIFT )
|
|
{
|
|
case 0x0: /* AND Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, GET_REGISTER(rd) & GET_REGISTER(rs) );
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x1: /* EOR Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, GET_REGISTER(rd) ^ GET_REGISTER(rs) );
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x2: /* LSL Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrd = GET_REGISTER(rd);
|
|
offs = GET_REGISTER(rs) & 0x000000ff;
|
|
if (offs > 0)
|
|
{
|
|
if ( offs < 32 )
|
|
{
|
|
SET_REGISTER( rd, rrd << offs );
|
|
if( rrd & ( 1 << ( 31 - ( offs - 1 ) ) ) )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
else if( offs == 32 )
|
|
{
|
|
SET_REGISTER( rd, 0 );
|
|
if( rrd & 1 )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SET_REGISTER( rd, 0 );
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x3: /* LSR Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrd = GET_REGISTER(rd);
|
|
offs = GET_REGISTER(rs) & 0x000000ff;
|
|
if (offs > 0)
|
|
{
|
|
if( offs < 32 )
|
|
{
|
|
SET_REGISTER( rd, rrd >> offs );
|
|
if( rrd & ( 1 << ( offs - 1 ) ) )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
else if( offs == 32 )
|
|
{
|
|
SET_REGISTER( rd, 0 );
|
|
if( rrd & 0x80000000 )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SET_REGISTER( rd, 0 );
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
}
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x4: /* ASR Rd, Rs */
|
|
{
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrs = GET_REGISTER(rs)&0xff;
|
|
rrd = GET_REGISTER(rd);
|
|
if (rrs != 0)
|
|
{
|
|
if (rrs >= 32)
|
|
{
|
|
if (rrd>>31)
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR | ARM7_CPSR_C);
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_C);
|
|
}
|
|
SET_REGISTER( rd, (GET_REGISTER(rd) & 0x80000000) ? 0xFFFFFFFF : 0x00000000 );
|
|
}
|
|
else
|
|
{
|
|
if ((rrd>>(rs-1))&1)
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR | ARM7_CPSR_C);
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_C);
|
|
}
|
|
SET_REGISTER( rd, (rrd & 0x80000000) ? ((0xFFFFFFFF<<(32-rrs)) | (rrd>>rrs)) : (rrd>>rrs));
|
|
}
|
|
}
|
|
ARM7_SetCPSR(GET_CPSR &~ (ARM7_CPSR_N | ARM7_CPSR_Z));
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0x5: /* ADC Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
op2=(GET_CPSR & ARM7_CPSR_C) ? 1 : 0;
|
|
rn=GET_REGISTER(rd) + GET_REGISTER(rs) + op2;
|
|
HandleThumbALUAddFlags( rn, GET_REGISTER(rd), ( GET_REGISTER(rs) ) ); //?
|
|
SET_REGISTER( rd, rn);
|
|
break;
|
|
case 0x6: /* SBC Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
op2=(GET_CPSR & ARM7_CPSR_C) ? 0 : 1;
|
|
rn=GET_REGISTER(rd) - GET_REGISTER(rs) - op2;
|
|
HandleThumbALUSubFlags( rn, GET_REGISTER(rd), ( GET_REGISTER(rs) ) ); //?
|
|
SET_REGISTER( rd, rn);
|
|
break;
|
|
case 0x7: /* ROR Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrd = GET_REGISTER(rd);
|
|
imm = GET_REGISTER(rs) & 0x0000001f;
|
|
SET_REGISTER( rd, ( rrd >> imm ) | ( rrd << ( 32 - imm ) ) );
|
|
if( rrd & ( 1 << ( imm - 1 ) ) )
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR | ARM7_CPSR_C );
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR( GET_CPSR &~ ARM7_CPSR_C );
|
|
}
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x8: /* TST Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) & GET_REGISTER(rs) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x9: /* NEG Rd, Rs - todo: check me */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rrs = GET_REGISTER(rs);
|
|
rn = 0 - rrs;
|
|
SET_REGISTER( rd, rn );
|
|
HandleThumbALUSubFlags( GET_REGISTER(rd), 0, rrs );
|
|
break;
|
|
case 0xa: /* CMP Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rn = GET_REGISTER(rd) - GET_REGISTER(rs);
|
|
HandleThumbALUSubFlags( rn, GET_REGISTER(rd), GET_REGISTER(rs) );
|
|
break;
|
|
case 0xb: /* CMN Rd, Rs - check flags, add dasm */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rn = GET_REGISTER(rd) + GET_REGISTER(rs);
|
|
HandleThumbALUAddFlags( rn, GET_REGISTER(rd), GET_REGISTER(rs) );
|
|
break;
|
|
case 0xc: /* ORR Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, GET_REGISTER(rd) | GET_REGISTER(rs) );
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0xd: /* MUL Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
rn = GET_REGISTER(rd) * GET_REGISTER(rs);
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
SET_REGISTER( rd, rn );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( rn ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0xe: /* BIC Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, GET_REGISTER(rd) & (~GET_REGISTER(rs)) );
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0xf: /* MVN Rd, Rs */
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
op2 = GET_REGISTER(rs);
|
|
SET_REGISTER( rd, ~op2 );
|
|
ARM7_SetCPSR( GET_CPSR &~ ( ARM7_CPSR_Z | ARM7_CPSR_N ) );
|
|
ARM7_SetCPSR( GET_CPSR | HandleALUNZFlags( GET_REGISTER(rd) ) );
|
|
R15 += 2;
|
|
break;
|
|
default:
|
|
printf("%08x: G4-0 Undefined Thumb instruction: %04x %x\n", pc, insn, ( insn & THUMB_ALUOP_TYPE ) >> THUMB_ALUOP_TYPE_SHIFT);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0x1:
|
|
switch( ( insn & THUMB_HIREG_OP ) >> THUMB_HIREG_OP_SHIFT )
|
|
{
|
|
case 0x0: /* ADD Rd, Rs */
|
|
rs = ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT;
|
|
rd = insn & THUMB_HIREG_RD;
|
|
switch( ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT )
|
|
{
|
|
case 0x1: /* ADD Rd, HRs */
|
|
SET_REGISTER( rd, GET_REGISTER(rd) + GET_REGISTER(rs+8) );
|
|
// emulate the effects of pre-fetch
|
|
if (rs == 7)
|
|
{
|
|
SET_REGISTER(rd, GET_REGISTER(rd) + 4);
|
|
}
|
|
break;
|
|
case 0x2: /* ADD HRd, Rs */
|
|
SET_REGISTER( rd+8, GET_REGISTER(rd+8) + GET_REGISTER(rs) );
|
|
if (rd == 7)
|
|
{
|
|
R15 += 2;
|
|
change_pc(R15);
|
|
}
|
|
break;
|
|
case 0x3: /* Add HRd, HRs */
|
|
SET_REGISTER( rd+8, GET_REGISTER(rd+8) + GET_REGISTER(rs+8) );
|
|
// emulate the effects of pre-fetch
|
|
if (rs == 7)
|
|
{
|
|
SET_REGISTER(rd+8, GET_REGISTER(rd+8) + 4);
|
|
}
|
|
if (rd == 7)
|
|
{
|
|
R15 += 2;
|
|
change_pc(R15);
|
|
}
|
|
break;
|
|
default:
|
|
printf("%08x: G4-1-0 Undefined Thumb instruction: %04x %x\n", pc, insn, ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT );
|
|
break;
|
|
}
|
|
R15 += 2;
|
|
break;
|
|
case 0x1: /* CMP */
|
|
switch( ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT )
|
|
{
|
|
case 0x0: /* CMP Rd, Rs */
|
|
rs = GET_REGISTER( ( ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT ) );
|
|
rd = GET_REGISTER( insn & THUMB_HIREG_RD );
|
|
rn = rd - rs;
|
|
HandleThumbALUSubFlags( rn, rd, rs );
|
|
break;
|
|
case 0x1: /* CMP Rd, Hs */
|
|
rs = GET_REGISTER( ( ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT ) + 8 );
|
|
rd = GET_REGISTER( insn & THUMB_HIREG_RD );
|
|
rn = rd - rs;
|
|
HandleThumbALUSubFlags( rn, rd, rs );
|
|
break;
|
|
case 0x2: /* CMP Hd, Rs */
|
|
rs = GET_REGISTER( ( ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT ) );
|
|
rd = GET_REGISTER( (insn & THUMB_HIREG_RD) + 8 );
|
|
rn = rd - rs;
|
|
HandleThumbALUSubFlags( rn, rd, rs );
|
|
break;
|
|
case 0x3: /* CMP Hd, Hs */
|
|
rs = GET_REGISTER( ( ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT ) + 8 );
|
|
rd = GET_REGISTER( (insn & THUMB_HIREG_RD) + 8 );
|
|
rn = rd - rs;
|
|
HandleThumbALUSubFlags( rn, rd, rs );
|
|
break;
|
|
default:
|
|
printf("%08x: G4-1 Undefined Thumb instruction: %04x %x\n", pc, insn, ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0x2: /* MOV */
|
|
switch( ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT )
|
|
{
|
|
case 0x1: // MOV Rd, Hs
|
|
rs = ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT;
|
|
rd = insn & THUMB_HIREG_RD;
|
|
if( rs == 7 )
|
|
{
|
|
SET_REGISTER( rd, GET_REGISTER(rs + 8) + 4 );
|
|
}
|
|
else
|
|
{
|
|
SET_REGISTER( rd, GET_REGISTER(rs + 8) );
|
|
}
|
|
R15 += 2;
|
|
break;
|
|
case 0x2: // MOV Hd, Rs
|
|
rs = ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT;
|
|
rd = insn & THUMB_HIREG_RD;
|
|
SET_REGISTER( rd + 8, GET_REGISTER(rs) );
|
|
if( rd != 7 )
|
|
{
|
|
R15 += 2;
|
|
}
|
|
else
|
|
{
|
|
R15 &= ~1;
|
|
change_pc(R15);
|
|
}
|
|
break;
|
|
case 0x3: // MOV Hd, Hs
|
|
rs = ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT;
|
|
rd = insn & THUMB_HIREG_RD;
|
|
if (rs == 7)
|
|
{
|
|
SET_REGISTER( rd + 8, GET_REGISTER(rs+8)+4 );
|
|
}
|
|
else
|
|
{
|
|
SET_REGISTER( rd + 8, GET_REGISTER(rs+8) );
|
|
}
|
|
if( rd != 7 )
|
|
{
|
|
R15 += 2;
|
|
}
|
|
|
|
if( rd == 7 )
|
|
{
|
|
R15 &= ~1;
|
|
change_pc(R15);
|
|
}
|
|
break;
|
|
default:
|
|
printf("%08x: G4-2 Undefined Thumb instruction: %04x (%x)\n", pc, insn, ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0x3:
|
|
switch( ( insn & THUMB_HIREG_H ) >> THUMB_HIREG_H_SHIFT )
|
|
{
|
|
case 0x0:
|
|
rd = ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT;
|
|
addr = GET_REGISTER(rd);
|
|
if( addr & 1 )
|
|
{
|
|
addr &= ~1;
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_T);
|
|
if( addr & 2 )
|
|
{
|
|
addr += 2;
|
|
}
|
|
}
|
|
R15 = addr;
|
|
break;
|
|
case 0x1:
|
|
addr = GET_REGISTER( ( ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT ) + 8 );
|
|
if( ( ( ( insn & THUMB_HIREG_RS ) >> THUMB_HIREG_RS_SHIFT ) + 8 ) == 15 )
|
|
{
|
|
addr += 2;
|
|
}
|
|
if( addr & 1 )
|
|
{
|
|
addr &= ~1;
|
|
}
|
|
else
|
|
{
|
|
ARM7_SetCPSR(GET_CPSR &~ ARM7_CPSR_T);
|
|
if( addr & 2 )
|
|
{
|
|
addr += 2;
|
|
}
|
|
}
|
|
R15 = addr;
|
|
break;
|
|
default:
|
|
printf("%08x: G4-3 Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
default:
|
|
printf("%08x: G4-x Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0x2:
|
|
case 0x3:
|
|
readword = arm7_read_32( ( R15 & ~2 ) + 4 + ( ( insn & THUMB_INSN_IMM ) << 2 ) );
|
|
SET_REGISTER( ( insn & THUMB_INSN_IMM_RD ) >> THUMB_INSN_IMM_RD_SHIFT, readword );
|
|
R15 += 2;
|
|
break;
|
|
default:
|
|
printf("%08x: G4-y Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0x5: /* LDR* STR* */
|
|
switch( ( insn & THUMB_GROUP5_TYPE ) >> THUMB_GROUP5_TYPE_SHIFT )
|
|
{
|
|
case 0x0: /* STR Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
arm7_write_32( addr, GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x1: /* STRH Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
arm7_write_16( addr, GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x2: /* STRB Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
arm7_write_8( addr, GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x3: /* LDSB Rd, [Rn, Rm] todo, add dasm */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
op2 = arm7_read_8( addr );
|
|
if( op2 & 0x00000080 )
|
|
{
|
|
op2 |= 0xffffff00;
|
|
}
|
|
SET_REGISTER( rd, op2 );
|
|
R15 += 2;
|
|
break;
|
|
case 0x4: /* LDR Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
op2 = arm7_read_32( addr );
|
|
SET_REGISTER( rd, op2 );
|
|
R15 += 2;
|
|
break;
|
|
case 0x5: /* LDRH Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
op2 = arm7_read_16( addr );
|
|
SET_REGISTER( rd, op2 );
|
|
R15 += 2;
|
|
break;
|
|
|
|
case 0x6: /* LDRB Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
op2 = arm7_read_8( addr );
|
|
SET_REGISTER( rd, op2 );
|
|
R15 += 2;
|
|
break;
|
|
case 0x7: /* LDSH Rd, [Rn, Rm] */
|
|
rm = ( insn & THUMB_GROUP5_RM ) >> THUMB_GROUP5_RM_SHIFT;
|
|
rn = ( insn & THUMB_GROUP5_RN ) >> THUMB_GROUP5_RN_SHIFT;
|
|
rd = ( insn & THUMB_GROUP5_RD ) >> THUMB_GROUP5_RD_SHIFT;
|
|
addr = GET_REGISTER(rn) + GET_REGISTER(rm);
|
|
op2 = arm7_read_16( addr );
|
|
if( op2 & 0x00008000 )
|
|
{
|
|
op2 |= 0xffff0000;
|
|
}
|
|
SET_REGISTER( rd, op2 );
|
|
R15 += 2;
|
|
break;
|
|
default:
|
|
printf("%08x: G5 Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0x6: /* Word Store w/ Immediate Offset */
|
|
if( insn & THUMB_LSOP_L ) /* Load */
|
|
{
|
|
rn = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = insn & THUMB_ADDSUB_RD;
|
|
offs = ( ( insn & THUMB_LSOP_OFFS ) >> THUMB_LSOP_OFFS_SHIFT ) << 2;
|
|
SET_REGISTER( rd, arm7_read_32(GET_REGISTER(rn) + offs) ); // fix
|
|
R15 += 2;
|
|
}
|
|
else /* Store */
|
|
{
|
|
rn = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = insn & THUMB_ADDSUB_RD;
|
|
offs = ( ( insn & THUMB_LSOP_OFFS ) >> THUMB_LSOP_OFFS_SHIFT ) << 2;
|
|
arm7_write_32( GET_REGISTER(rn) + offs, GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0x7: /* Byte Store w/ Immeidate Offset */
|
|
if( insn & THUMB_LSOP_L ) /* Load */
|
|
{
|
|
rn = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = insn & THUMB_ADDSUB_RD;
|
|
offs = ( insn & THUMB_LSOP_OFFS ) >> THUMB_LSOP_OFFS_SHIFT;
|
|
SET_REGISTER( rd, arm7_read_8( GET_REGISTER(rn) + offs ) );
|
|
R15 += 2;
|
|
}
|
|
else /* Store */
|
|
{
|
|
rn = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = insn & THUMB_ADDSUB_RD;
|
|
offs = ( insn & THUMB_LSOP_OFFS ) >> THUMB_LSOP_OFFS_SHIFT;
|
|
arm7_write_8( GET_REGISTER(rn) + offs, GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0x8: /* Load/Store Halfword */
|
|
if( insn & THUMB_HALFOP_L ) /* Load */
|
|
{
|
|
imm = ( insn & THUMB_HALFOP_OFFS ) >> THUMB_HALFOP_OFFS_SHIFT;
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
SET_REGISTER( rd, arm7_read_16( GET_REGISTER(rs) + ( imm << 1 ) ) );
|
|
R15 += 2;
|
|
}
|
|
else /* Store */
|
|
{
|
|
imm = ( insn & THUMB_HALFOP_OFFS ) >> THUMB_HALFOP_OFFS_SHIFT;
|
|
rs = ( insn & THUMB_ADDSUB_RS ) >> THUMB_ADDSUB_RS_SHIFT;
|
|
rd = ( insn & THUMB_ADDSUB_RD ) >> THUMB_ADDSUB_RD_SHIFT;
|
|
arm7_write_16( GET_REGISTER(rs) + ( imm << 1 ), GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0x9: /* Stack-Relative Load/Store */
|
|
if( insn & THUMB_STACKOP_L )
|
|
{
|
|
rd = ( insn & THUMB_STACKOP_RD ) >> THUMB_STACKOP_RD_SHIFT;
|
|
offs = (UINT8)( insn & THUMB_INSN_IMM );
|
|
readword = arm7_read_32( GET_REGISTER(13) + ( (UINT32)offs << 2 ) );
|
|
SET_REGISTER( rd, readword );
|
|
R15 += 2;
|
|
}
|
|
else
|
|
{
|
|
rd = ( insn & THUMB_STACKOP_RD ) >> THUMB_STACKOP_RD_SHIFT;
|
|
offs = (UINT8)( insn & THUMB_INSN_IMM );
|
|
arm7_write_32( GET_REGISTER(13) + ( (UINT32)offs << 2 ), GET_REGISTER(rd) );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0xa: /* Get relative address */
|
|
if( insn & THUMB_RELADDR_SP ) /* ADD Rd, SP, #nn */
|
|
{
|
|
rd = ( insn & THUMB_RELADDR_RD ) >> THUMB_RELADDR_RD_SHIFT;
|
|
offs = (UINT8)( insn & THUMB_INSN_IMM ) << 2;
|
|
SET_REGISTER( rd, GET_REGISTER(13) + offs );
|
|
R15 += 2;
|
|
}
|
|
else /* ADD Rd, PC, #nn */
|
|
{
|
|
rd = ( insn & THUMB_RELADDR_RD ) >> THUMB_RELADDR_RD_SHIFT;
|
|
offs = (UINT8)( insn & THUMB_INSN_IMM ) << 2;
|
|
SET_REGISTER( rd, ( ( R15 + 4 ) & ~2 ) + offs );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0xb: /* Stack-Related Opcodes */
|
|
switch( ( insn & THUMB_STACKOP_TYPE ) >> THUMB_STACKOP_TYPE_SHIFT )
|
|
{
|
|
case 0x0: /* ADD SP, #imm */
|
|
addr = ( insn & THUMB_INSN_IMM );
|
|
addr &= ~THUMB_INSN_IMM_S;
|
|
SET_REGISTER( 13, GET_REGISTER(13) + ( ( insn & THUMB_INSN_IMM_S ) ? -( addr << 2 ) : ( addr << 2 ) ) );
|
|
R15 += 2;
|
|
break;
|
|
case 0x4: /* PUSH {Rlist} */
|
|
for( offs = 7; offs >= 0; offs-- )
|
|
{
|
|
if( insn & ( 1 << offs ) )
|
|
{
|
|
SET_REGISTER( 13, GET_REGISTER(13) - 4 );
|
|
arm7_write_32( GET_REGISTER(13), GET_REGISTER(offs) );
|
|
}
|
|
}
|
|
R15 += 2;
|
|
break;
|
|
case 0x5: /* PUSH {Rlist}{LR} */
|
|
SET_REGISTER( 13, GET_REGISTER(13) - 4 );
|
|
arm7_write_32( GET_REGISTER(13), GET_REGISTER(14) );
|
|
for( offs = 7; offs >= 0; offs-- )
|
|
{
|
|
if( insn & ( 1 << offs ) )
|
|
{
|
|
SET_REGISTER( 13, GET_REGISTER(13) - 4 );
|
|
arm7_write_32( GET_REGISTER(13), GET_REGISTER(offs) );
|
|
}
|
|
}
|
|
R15 += 2;
|
|
break;
|
|
case 0xc: /* POP {Rlist} */
|
|
for( offs = 0; offs < 8; offs++ )
|
|
{
|
|
if( insn & ( 1 << offs ) )
|
|
{
|
|
SET_REGISTER( offs, arm7_read_32( GET_REGISTER(13) ) );
|
|
SET_REGISTER( 13, GET_REGISTER(13) + 4 );
|
|
}
|
|
}
|
|
R15 += 2;
|
|
break;
|
|
case 0xd: /* POP {Rlist}{PC} */
|
|
for( offs = 0; offs < 8; offs++ )
|
|
{
|
|
if( insn & ( 1 << offs ) )
|
|
{
|
|
SET_REGISTER( offs, arm7_read_32( GET_REGISTER(13) ) );
|
|
SET_REGISTER( 13, GET_REGISTER(13) + 4 );
|
|
}
|
|
}
|
|
R15 = arm7_read_32( GET_REGISTER(13) ) & ~1;
|
|
SET_REGISTER( 13, GET_REGISTER(13) + 4 );
|
|
break;
|
|
default:
|
|
printf("%08x: Gb Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
break;
|
|
case 0xc: /* Multiple Load/Store */
|
|
if( insn & THUMB_MULTLS ) /* Load */
|
|
{
|
|
rd = ( insn & THUMB_MULTLS_BASE ) >> THUMB_MULTLS_BASE_SHIFT;
|
|
for( offs = 0; offs < 8; offs++ )
|
|
{
|
|
if( insn & ( 1 << offs ) )
|
|
{
|
|
SET_REGISTER( offs, arm7_read_32( (GET_REGISTER(rd)&0xfffffffc) ) );
|
|
SET_REGISTER( rd, GET_REGISTER(rd) + 4 );
|
|
}
|
|
}
|
|
R15 += 2;
|
|
}
|
|
else /* Store */
|
|
{
|
|
rd = ( insn & THUMB_MULTLS_BASE ) >> THUMB_MULTLS_BASE_SHIFT;
|
|
for( offs = 0; offs < 8; offs++ )
|
|
{
|
|
if( insn & ( 1 << offs ) )
|
|
{
|
|
arm7_write_32( (GET_REGISTER(rd)&0xfffffffc), GET_REGISTER(offs) );
|
|
SET_REGISTER( rd, GET_REGISTER(rd) + 4 );
|
|
}
|
|
}
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case 0xd: /* Conditional Branch */
|
|
offs = (INT8)( insn & THUMB_INSN_IMM );
|
|
switch( ( insn & THUMB_COND_TYPE ) >> THUMB_COND_TYPE_SHIFT )
|
|
{
|
|
case COND_EQ:
|
|
if( Z_IS_SET(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_NE:
|
|
if( Z_IS_CLEAR(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_CS:
|
|
if( C_IS_SET(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_CC:
|
|
if( C_IS_CLEAR(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_MI:
|
|
if( N_IS_SET(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_PL:
|
|
if( N_IS_CLEAR(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_VS:
|
|
if( V_IS_SET(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_VC:
|
|
if( V_IS_CLEAR(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_HI:
|
|
if( C_IS_SET(GET_CPSR) && Z_IS_CLEAR(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_LS:
|
|
if( C_IS_CLEAR(GET_CPSR) || Z_IS_SET(GET_CPSR) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_GE:
|
|
if( !(GET_CPSR & ARM7_CPSR_N) == !(GET_CPSR & ARM7_CPSR_V) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_LT:
|
|
if( !(GET_CPSR & ARM7_CPSR_N) != !(GET_CPSR & ARM7_CPSR_V) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_GT:
|
|
if( Z_IS_CLEAR(GET_CPSR) && ( !(GET_CPSR & ARM7_CPSR_N) == !(GET_CPSR & ARM7_CPSR_V) ) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_LE:
|
|
if( Z_IS_SET(GET_CPSR) || ( !(GET_CPSR & ARM7_CPSR_N) != !(GET_CPSR & ARM7_CPSR_V) ) )
|
|
{
|
|
R15 += 4 + (offs << 1);
|
|
}
|
|
else
|
|
{
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
case COND_AL:
|
|
printf("%08x: Undefined Thumb instruction: %04x (ARM9 reserved)\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
|
|
case COND_NV: // SWI (this is sort of a "hole" in the opcode encoding)
|
|
ARM7_SetSWI();
|
|
// R15 -= 4;
|
|
break;
|
|
}
|
|
break;
|
|
case 0xe: /* B #offs */
|
|
if( insn & THUMB_BLOP_LO )
|
|
{
|
|
addr = GET_REGISTER(14);
|
|
addr += ( insn & THUMB_BLOP_OFFS ) << 1;
|
|
addr &= 0xfffffffc;
|
|
SET_REGISTER( 14, ( R15 + 4 ) | 1 );
|
|
R15 = addr;
|
|
}
|
|
else
|
|
{
|
|
offs = ( insn & THUMB_BRANCH_OFFS ) << 1;
|
|
if( offs & 0x00000800 )
|
|
{
|
|
offs |= 0xfffff800;
|
|
}
|
|
R15 += 4 + offs;
|
|
}
|
|
break;
|
|
case 0xf: /* BL */
|
|
if( insn & THUMB_BLOP_LO )
|
|
{
|
|
addr = GET_REGISTER(14);
|
|
addr += ( insn & THUMB_BLOP_OFFS ) << 1;
|
|
SET_REGISTER( 14, ( R15 + 2 ) | 1 );
|
|
R15 = addr;
|
|
}
|
|
else
|
|
{
|
|
addr = ( insn & THUMB_BLOP_OFFS ) << 12;
|
|
if( addr & ( 1 << 22 ) )
|
|
{
|
|
addr |= 0xff800000;
|
|
}
|
|
addr += R15 + 4;
|
|
SET_REGISTER( 14, addr );
|
|
R15 += 2;
|
|
}
|
|
break;
|
|
default:
|
|
printf("%08x: Undefined Thumb instruction: %04x\n", pc, insn);
|
|
R15 += 2;
|
|
break;
|
|
}
|
|
|
|
return cycles;
|
|
}
|