/*****************************************************************************
 *
 *	 arm7core.c
 *	 Portable ARM7TDMI Core Emulator
 *
 *	 Copyright (c) 2004 Steve Ellenoff, all rights reserved.
 *
 *	 - This source code is released as freeware for non-commercial purposes.
 *	 - You are free to use and redistribute this code in modified or
 *	   unmodified form, provided you list me in the credits.
 *	 - If you modify this source code, you must add a notice to each modified
 *	   source file that it has been changed.  If you're a nice person, you
 *	   will clearly mark each change too.  :)
 *	 - If you wish to use this for commercial purposes, please contact me at
 *	   sellenoff@hotmail.com
 *	 - The author of this copywritten work reserves the right to change the
 *	   terms of its usage and license at any time, including retroactively
 *	 - This entire notice must remain in the source code.
 *
 *	This work is based on:
 *  #1) 'Atmel Corporation ARM7TDMI (Thumb) Datasheet - January 1999'
 *  #2) Arm 2/3/6 emulator By Bryan McPhail (bmcphail@tendril.co.uk) and Phil Stroffolino (MAME CORE 0.76)
 *
 *****************************************************************************/

/******************************************************************************
 *  Notes:

    **This core comes from my AT91 cpu core contributed to PinMAME, 
	  but with all the AT91 specific junk removed, 
	  which leaves just the ARM7TDMI core itself. I further removed the CPU specific MAME stuff
	  so you just have the actual ARM7 core itself, since many cpu's incorporate an ARM7 core, but add on
	  many cpu specific functionality.

	  Therefore, to use the core, you simpy include this file along with the .h file into your own cpu specific
	  implementation, and therefore, this file shouldn't be compiled as part of your project directly.
	  Additionally, you will need to include arm7exec.c in your cpu's execute routine.

	  For better or for worse, the code itself is very much intact from it's arm 2/3/6 origins from 
	  Bryan & Phil's work. I contemplated merging it in, but thought the fact that the CPSR is
	  no longer part of the PC was enough of a change to make it annoying to merge.
	**

	Coprocessor functions are heavily implementation specific, so callback handlers are used to allow the 
	implementation to handle the functionality. Custom DASM handlers are included as well to allow the DASM
	output to be tailored to the co-proc implementation details.

	Todo:
	Thumb mode support not yet implemented. 26 bit compatibility mode not implemented.
	Data Processing opcodes need cycle count adjustments (see page 194 of ARM7TDMI manual for instruction timing summary)
	Multi-emulated cpu support untested, but probably will not work too well, as no effort was made to code for more than 1.
	Could not find info on what the TEQP opcode is from page 44..
	I have no idea if user bank switching is right, as I don't fully understand it's use.
	Search for Todo: tags for remaining items not done.
	

	Differences from Arm 2/3 (6 also?)
	-Full 32 bit address support
	-PC no longer contains CPSR information, CPSR is own register now
	-New register SPSR to store previous contents of CPSR (this register is banked in many modes)
	-New opcodes for CPSR transfer, Long Multiplication, Co-Processor support, and some others
	-User Bank Mode transfer using certain flags which were previously unallowed (LDM/STM with S Bit & R15)
	-New operation modes? (unconfirmed)

	Based heavily on arm core from MAME 0.76:
    *****************************************
	ARM 2/3/6 Emulation

	Todo:
	Software interrupts unverified (nothing uses them so far, but they should be ok)
	Timing - Currently very approximated, nothing relies on proper timing so far.
	IRQ timing not yet correct (again, nothing is affected by this so far).

	By Bryan McPhail (bmcphail@tendril.co.uk) and Phil Stroffolino
*****************************************************************************/
#define ARM7_DEBUG_CORE 0

#if 0
#define LOG(x) printf x
#else
#define LOG(x) logerror x
#endif

/* Prototypes */

//SJE: should these be inline? or are they too big to see any benefit?

static void HandleCoProcDO(data32_t insn);
static void HandleCoProcRT(data32_t insn);
static void HandleCoProcDT(data32_t insn);
static void HandleHalfWordDT(data32_t insn);
static void HandleSwap(data32_t insn);
static void HandlePSRTransfer( data32_t insn );
static void HandleALU( data32_t insn);
static void HandleMul( data32_t insn);
static void HandleUMulLong( data32_t insn);
static void HandleSMulLong( data32_t insn);
//static void HandleBranch( data32_t insn);
INLINE void HandleBranch( data32_t insn);		//pretty short, so inline should be ok
static void HandleMemSingle( data32_t insn);
static void HandleMemBlock( data32_t insn);
static data32_t decodeShift( data32_t insn, data32_t *pCarry);
INLINE void SwitchMode( int );
static void arm7_check_irq_state(void);

INLINE void arm7_cpu_write32( int addr, data32_t data );
INLINE void arm7_cpu_write16( int addr, data16_t data );
INLINE void arm7_cpu_write8( int addr, data8_t data );
INLINE data32_t arm7_cpu_read32( int addr );
INLINE data16_t arm7_cpu_read16( int addr );
INLINE data8_t arm7_cpu_read8( offs_t addr );

/***************************************************************************
 * Default Memory Handlers 
 ***************************************************************************/
INLINE void arm7_cpu_write32( int addr, data32_t data )
{
	//Call normal 32 bit handler
	cpu_writemem32ledw_dword(addr,data);

	/* Unaligned writes are treated as normal writes */
	#if ARM7_DEBUG_CORE
		if(addr&3)
			LOG(("%08x: Unaligned write %08x\n",R15,addr));
	#endif
}


INLINE void arm7_cpu_write16( int addr, data16_t data )
{
	//Call normal 16 bit handler ( for 32 bit cpu )
	cpu_writemem32ledw_word(addr,data);
}

INLINE void arm7_cpu_write8( int addr, data8_t data )
{
	//Call normal 8 bit handler ( for 32 bit cpu )
	cpu_writemem32ledw(addr,data);
}

INLINE data32_t arm7_cpu_read32( int addr )
{
	data32_t result = 0;

	//Handle through normal 32 bit handler
	result = cpu_readmem32ledw_dword(addr);

	/* Unaligned reads rotate the word, they never combine words */
	if (addr&3) {
		#if ARM7_DEBUG_CORE
			if(addr&1)
				LOG(("%08x: Unaligned byte read %08x\n",R15,addr));
		#endif

		if ((addr&3)==3)
			return ((result&0x000000ff)<<24)|((result&0xffffff00)>> 8);
		if ((addr&3)==2)
			return ((result&0x0000ffff)<<16)|((result&0xffff0000)>>16);
		if ((addr&3)==1)
			return ((result&0x00ffffff)<< 8)|((result&0xff000000)>>24);
	}
	return result;
}

INLINE data16_t arm7_cpu_read16( int addr )
{
	if(addr&3)
	{
		int val = addr & 3;
		if(val != 2)
			LOG(("%08x: MISALIGNED half word read @ %08x:\n",R15,addr));
	}

	//Handle through normal 32 bit handler ( for 32 bit cpu )
	return cpu_readmem32ledw_word(addr);
}

INLINE data8_t arm7_cpu_read8( offs_t addr )
{
	//Handle through normal 8 bit handler ( for 32 bit cpu )
	return cpu_readmem32ledw(addr);
}

/***************
 * helper funcs
 ***************/

/* Set NZCV flags for ADDS / SUBS */
#define HandleALUAddFlags(rd, rn, op2) \
  if (insn & INSN_S) \
    SET_CPSR( \
      ((GET_CPSR &~ (N_MASK | Z_MASK | V_MASK | C_MASK)) \
      | (((!SIGN_BITS_DIFFER(rn, op2)) && SIGN_BITS_DIFFER(rn, rd)) \
          << V_BIT) \
      | (((~(rn)) < (op2)) << C_BIT) \
      | HandleALUNZFlags(rd))); \
  R15 += 4;

#define HandleALUSubFlags(rd, rn, op2) \
  if (insn & INSN_S) \
    SET_CPSR( \
      ((GET_CPSR &~ (N_MASK | Z_MASK | V_MASK | C_MASK)) \
      | ((SIGN_BITS_DIFFER(rn, op2) && SIGN_BITS_DIFFER(rn, rd)) \
          << V_BIT) \
      | (((op2) <= (rn)) << C_BIT) \
      | HandleALUNZFlags(rd))); \
  R15 += 4;

/* Set NZC flags for logical operations. */

//This macro (which I didn't write) - doesn't make it obvious that the SIGN BIT = 31, just as the N Bit does,
//therfore, N is set by default
#define HandleALUNZFlags(rd) \
  (((rd) & SIGN_BIT) | ((!(rd)) << Z_BIT))


//Long ALU Functions use bit 63 
#define HandleLongALUNZFlags(rd) \
  ((((rd) & ((UINT64)1<<63))>>32) | ((!(rd)) << Z_BIT))

#define HandleALULogicalFlags(rd, sc) \
  if (insn & INSN_S) \
    SET_CPSR( ((GET_CPSR &~ (N_MASK | Z_MASK | C_MASK)) \
                     | HandleALUNZFlags(rd) \
                     | (((sc) != 0) << C_BIT)));\
  R15 += 4;

//convert cpsr mode num into to text
static const char modetext[ARM7_NUM_MODES][5] = {
		 "USER","FIRQ","IRQ","SVC","ILL1","ILL2","ILL3","ABT",
		 "ILL4","ILL5","ILL6","UND","ILL7","ILL8","ILL9","SYS"
};
static const char* GetModeText( int cpsr )
{
	return modetext[cpsr & MODE_FLAG];
}

#define GetRegister(rIndex) ARMREG(sRegisterTable[GET_MODE][rIndex])
#define SetRegister(rIndex,value) ARMREG(sRegisterTable[GET_MODE][rIndex]) = value

//I could prob. convert to macro, but Switchmode shouldn't occur that often in emulated code..
INLINE void SwitchMode (int cpsr_mode_val)
{
	data32_t cspr = GET_CPSR & ~MODE_FLAG;
	SET_CPSR(cspr | cpsr_mode_val);
}


/* Decodes an Op2-style shifted-register form.  If @carry@ is non-zero the
 * shifter carry output will manifest itself as @*carry == 0@ for carry clear
 * and @*carry != 0@ for carry set.

   SJE: Rules: 
   IF RC = 256, Result = no shift.
   LSL   0   = Result = RM, Carry = Old Contents of CPSR C Bit
   LSL(0,31) = Result shifted, least significant bit is in carry out
   LSL  32   = Result of 0, Carry = Bit 0 of RM
   LSL >32   = Result of 0, Carry out 0
   LSR   0   = LSR 32 (see below)
   LSR  32   = Result of 0, Carry = Bit 31 of RM
   LSR >32   = Result of 0, Carry out 0
   ASR >=32  = ENTIRE Result = bit 31 of RM
   ROR  32   = Result = RM, Carry = Bit 31 of RM
   ROR >32   = Same result as ROR n-32 until amount in range of 1-32 then follow rules
*/

static data32_t decodeShift( data32_t insn, data32_t *pCarry)
{
	data32_t k	= (insn & INSN_OP2_SHIFT) >> INSN_OP2_SHIFT_SHIFT;	//Bits 11-7
	data32_t rm	= GET_REGISTER( insn & INSN_OP2_RM );
	data32_t t	= (insn & INSN_OP2_SHIFT_TYPE) >> INSN_OP2_SHIFT_TYPE_SHIFT;

	if ((insn & INSN_OP2_RM)==0xf) {
		/* If hardwired shift, then PC is 8 bytes ahead, else if register shift
		is used, then 12 bytes - TODO?? */
		rm+=8;
	}

	/* All shift types ending in 1 are Rk, not #k */
	if( t & 1 )
	{
//		LOG(("%08x:  RegShift %02x %02x\n",R15, k>>1,GET_REGISTER(k >> 1)));
		#if ARM7_DEBUG_CORE
			if((insn&0x80)==0x80)
				LOG(("%08x:  RegShift ERROR (p36)\n",R15));
		#endif

		//see p35 for check on this
		//k = GET_REGISTER(k >> 1)&0x1f;

		//Keep only the bottom 8 bits for a Register Shift
		k = GET_REGISTER(k >> 1)&0xff;

		if( k == 0 ) /* Register shift by 0 is a no-op */
		{
//			LOG(("%08x:  NO-OP Regshift\n",R15));
			if (pCarry) *pCarry = GET_CPSR & C_MASK;
			return rm;
		}
	}
	/* Decode the shift type and perform the shift */
	switch (t >> 1)
	{
	case 0:						/* LSL */
		//LSL  32   = Result of 0, Carry = Bit 0 of RM
		//LSL >32   = Result of 0, Carry out 0
		if(k>=32) 
		{
			if(pCarry)	*pCarry = (k==32)?rm&1:0;
			return 0;
		}
		else
		{
			if (pCarry)
			{
			//LSL      0   = Result = RM, Carry = Old Contents of CPSR C Bit
			//LSL (0,31)   = Result shifted, least significant bit is in carry out
			*pCarry = k ? (rm & (1 << (32 - k))) : (GET_CPSR & C_MASK);
			}
			return k ? LSL(rm, k) : rm;
		}
		break;

	case 1:			       			/* LSR */
		if (k == 0 || k == 32)
		{
			if (pCarry) *pCarry = rm & SIGN_BIT;
			return 0;
		}
		else if (k > 32)
		{
			if (pCarry) *pCarry = 0;
			return 0;
		}
		else
		{
			if (pCarry) *pCarry = (rm & (1 << (k - 1)));
			return LSR(rm, k);
		}
		break;

	case 2:						/* ASR */
		if (k == 0 || k > 32)
			k = 32;

		if (pCarry) *pCarry = (rm & (1 << (k - 1)));
		if (k >= 32)
			return rm & SIGN_BIT ? 0xffffffffu : 0;
		else
		{
			if (rm & SIGN_BIT)
				return LSR(rm, k) | (0xffffffffu << (32 - k));
			else
				return LSR(rm, k);
		}
		break;

	case 3:						/* ROR and RRX */
		if (k)
		{
			while (k > 32) k -= 32;
			if (pCarry) *pCarry = rm & SIGN_BIT;
			return ROR(rm, k);
		}
		else
		{
			if (pCarry) *pCarry = (rm & 1);
			return LSR(rm, 1) | ((GET_CPSR & C_MASK) << 2);
		}
		break;
	}

	LOG(("%08x: Decodeshift error\n",R15));
	return 0;
} /* decodeShift */


static int loadInc ( data32_t pat, data32_t rbv, data32_t s)
{
	int i,result;

	result = 0;
	for( i=0; i<16; i++ )
	{
		if( (pat>>i)&1 )
		{
			if (i==15) {
				if (s) /* Pull full contents from stack */
					SET_REGISTER( 15, READ32(rbv+=4) );
				else /* Pull only address, preserve mode & status flags */
					SET_REGISTER( 15, READ32(rbv+=4) );
			} else
				SET_REGISTER( i, READ32(rbv+=4) );

			result++;
		}
	}
	return result;
}

static int loadDec( data32_t pat, data32_t rbv, data32_t s)
{
	int i,result;

	result = 0;
	for( i=15; i>=0; i-- )
	{
		if( (pat>>i)&1 )
		{
			if (i==15) {
				if (s) /* Pull full contents from stack */
					SET_REGISTER( 15, READ32(rbv-=4) );
				else /* Pull only address, preserve mode & status flags */
					SET_REGISTER( 15, READ32(rbv-=4) );
			}
			else
				SET_REGISTER( i, READ32(rbv -=4) );
			result++;
		}
	}
	return result;
}

static int storeInc( data32_t pat, data32_t rbv)
{
	int i,result;

	result = 0;
	for( i=0; i<16; i++ )
	{
		if( (pat>>i)&1 )
		{
			#if ARM7_DEBUG_CORE
				if(i==15) /* R15 is plus 12 from address of STM */
					LOG(("%08x: StoreInc on R15\n",R15));
			#endif
			WRITE32( rbv += 4, GET_REGISTER(i) );
			result++;
		}
	}
	return result;
} /* storeInc */

static int storeDec( data32_t pat, data32_t rbv)
{
	int i,result;

	result = 0;
	for( i=15; i>=0; i-- )
	{
		if( (pat>>i)&1 )
		{
			#if ARM7_DEBUG_CORE
				if(i==15) /* R15 is plus 12 from address of STM */
					LOG(("%08x: StoreDec on R15\n",R15));
			#endif
			WRITE32( rbv -= 4, GET_REGISTER(i) );
			result++;
		}
	}
	return result;
} /* storeDec */

/***************************************************************************
 *                            Main CPU Funcs
 ***************************************************************************/

//CPU INIT
static void arm7_core_init(const char *cpuname)
{
	int cpu = cpu_getactivecpu(),i;
	char buf[8];
	for (i=0; i<kNumRegisters; i++) {
		sprintf(buf,"R%d",i);
		state_save_register_UINT32(cpuname, cpu, buf, &ARMREG(i), 4);
	}
	state_save_register_UINT8(cpuname, cpu, "IRQ", &ARM7.pendingIrq, 1);
	state_save_register_UINT8(cpuname, cpu, "FIQ", &ARM7.pendingFiq, 1);
	state_save_register_UINT8(cpuname, cpu, "ABTD", &ARM7.pendingAbtD, 1);
	state_save_register_UINT8(cpuname, cpu, "ABTP", &ARM7.pendingAbtP, 1);
	state_save_register_UINT8(cpuname, cpu, "UND", &ARM7.pendingUnd, 1);
	state_save_register_UINT8(cpuname, cpu, "SWI", &ARM7.pendingSwi, 1);
}

//CPU RESET
static void arm7_core_reset(void *param)
{
	memset(&ARM7, 0, sizeof(ARM7));

	/* start up in SVC mode with interrupts disabled. */
	SwitchMode(eARM7_MODE_SVC);
	SET_CPSR(GET_CPSR | I_MASK | F_MASK);
	R15 = 0;
//change_pc32lew(R15);
}

//Execute used to be here.. moved to separate file (arm7exec.c) to be included by cpu cores separately

//CPU CHECK IRQ STATE
//Note: couldn't find any exact cycle counts for most of these exceptions
static void arm7_check_irq_state(void)
{
	data32_t cpsr = GET_CPSR;	/* save current CPSR */
	data32_t pc = R15+4;		/* save old pc (already incremented in pipeline) */;

	/* Exception priorities:

		Reset
		Data abort
		FIRQ
		IRQ
		Prefetch abort
		Undefined instruction
		Software Interrupt
	*/

	//Data Abort
	if (ARM7.pendingAbtD) {
		SwitchMode(eARM7_MODE_ABT);				/* Set ABT mode so PC is saved to correct R14 bank */
		SET_REGISTER( 14, pc );					/* save PC to R14 */
		SET_REGISTER( SPSR, cpsr );				/* Save current CPSR */
		R15 = 0x10;								/* IRQ Vector address */
		ARM7.pendingAbtD = 0;
		return;
	}

	//FIRQ
	if (ARM7.pendingFiq && (cpsr & F_MASK)==0) {
		SwitchMode(eARM7_MODE_FIQ);				/* Set FIQ mode so PC is saved to correct R14 bank */
		SET_REGISTER( 14, pc );					/* save PC to R14 */
		SET_REGISTER( SPSR, cpsr );				/* Save current CPSR */
		SET_CPSR(GET_CPSR | I_MASK | F_MASK);	/* Mask both IRQ & FIRQ*/
		R15 = 0x1c;								/* IRQ Vector address */
		return;
	}

	//IRQ
	if (ARM7.pendingIrq && (cpsr & I_MASK)==0) {
		SwitchMode(eARM7_MODE_IRQ);				/* Set IRQ mode so PC is saved to correct R14 bank */
		SET_REGISTER( 14, pc );					/* save PC to R14 */
		SET_REGISTER( SPSR, cpsr );				/* Save current CPSR */
		SET_CPSR(GET_CPSR | I_MASK);			/* Mask IRQ */
		R15 = 0x18;								/* IRQ Vector address */
		return;
	}

	//Prefetch Abort
	if (ARM7.pendingAbtP) {
		SwitchMode(eARM7_MODE_ABT);				/* Set ABT mode so PC is saved to correct R14 bank */
		SET_REGISTER( 14, pc );					/* save PC to R14 */
		SET_REGISTER( SPSR, cpsr );				/* Save current CPSR */
		R15 = 0x0c;								/* IRQ Vector address */
		ARM7.pendingAbtP = 0;
		return;
	}

	//Undefined instruction
	if (ARM7.pendingUnd) {
		SwitchMode(eARM7_MODE_UND);				/* Set UND mode so PC is saved to correct R14 bank */
		SET_REGISTER( 14, pc );					/* save PC to R14 */
		SET_REGISTER( SPSR, cpsr );				/* Save current CPSR */
		R15 = 0x04;								/* IRQ Vector address */
		ARM7.pendingUnd = 0;
		return;
	}

	//Software Interrupt
	if (ARM7.pendingSwi) {
		SwitchMode(eARM7_MODE_SVC);				/* Set SVC mode so PC is saved to correct R14 bank */
		SET_REGISTER( 14, pc );					/* save PC to R14 */
		SET_REGISTER( SPSR, cpsr );				/* Save current CPSR */
		R15 = 0x08;								/* IRQ Vector address */
		ARM7.pendingSwi = 0;
		return;
	}
}

//CPU - SET IRQ LINE
static void arm7_core_set_irq_line(int irqline, int state)
{
	switch (irqline) {

	case ARM7_IRQ_LINE: /* IRQ */
		ARM7.pendingIrq= (state & 1);
		break;

	case ARM7_FIRQ_LINE: /* FIRQ */
		ARM7.pendingFiq= (state & 1);
		break;

	case ARM7_ABORT_EXCEPTION:
		ARM7.pendingAbtD= (state & 1);
		break;
	case ARM7_ABORT_PREFETCH_EXCEPTION:
		ARM7.pendingAbtP= (state & 1);
		break;

	case ARM7_UNDEFINE_EXCEPTION:
		ARM7.pendingUnd= (state & 1);
		break;
	}

	ARM7_CHECKIRQ;
}

/***************************************************************************
 *                            OPCODE HANDLING
 ***************************************************************************/

// Co-Processor Data Operation
static void HandleCoProcDO(data32_t insn)
{
	// This instruction simply instructs the co-processor to do something, no data is returned to ARM7 core
	if(arm7_coproc_do_callback)
		arm7_coproc_do_callback(insn,0,0);		//simply pass entire opcode to callback - since data format is actually dependent on co-proc implementation
	else
		LOG(("%08x: Co-Processor Data Operation executed, but no callback defined!\n",R15));
}

//Co-Processor Register Transfer - To/From Arm to Co-Proc
static void HandleCoProcRT(data32_t insn)
{
	
	/* xxxx 1110 oooL nnnn dddd cccc ppp1 mmmm */

	// Load (MRC) data from Co-Proc to ARM7 register
	if( insn&0x00100000 )		//Bit 20 = Load or Store
		{
			if(arm7_coproc_rt_r_callback)
			{
				data32_t res = arm7_coproc_rt_r_callback(insn,0);	//RT Read handler must parse opcode & return appropriate result
				SET_REGISTER((insn>>12)&0xf,res);
			}
			else
				LOG(("%08x: Co-Processor Register Transfer executed, but no RT Read callback defined!\n",R15));
		}
	// Store (MCR) data from ARM7 to Co-Proc register
	else
		{
		if(arm7_coproc_rt_r_callback)
			arm7_coproc_rt_w_callback(insn,GET_REGISTER((insn>>12)&0xf),0);
		else
			LOG(("%08x: Co-Processor Register Transfer executed, but no RT Write callback defined!\n",R15));
		}
}

/*Data Transfer - To/From Arm to Co-Proc
   Loading or Storing, the co-proc function is responsible to read/write from the base register supplied + offset
   8 bit immediate value Base Offset address is << 2 to get the actual #
 
  issues - #1 - the co-proc function, needs direct access to memory reads or writes (ie, so we must send a pointer to a func)
         - #2 - the co-proc may adjust the base address (especially if it reads more than 1 word), so a pointer to the register must be used
                but the old value of the register must be restored if write back is not set..
		 - #3 - when post incrementing is used, it's up to the co-proc func. to add the offset, since the transfer
		        address supplied in that case, is simply the base. I suppose this is irrelevant if write back not set
				but if co-proc reads multiple address, it must handle the offset adjustment itself.
*/
//todo: test with valid instructions
static void HandleCoProcDT(data32_t insn)
{
	data32_t rn = (insn>>16)&0xf;
	data32_t rnv = GET_REGISTER(rn);	// Get Address Value stored from Rn 
	data32_t ornv = rnv;				// Keep value of Rn 
	data32_t off = (insn&0xff)<<2;		// Offset is << 2 according to manual
	data32_t* prn = &ARMREG(rn);		// Pointer to our register, so it can be changed in the callback

	//Pointers to read32/write32 functions
	void (*write32)(int addr, data32_t data);
	data32_t (*read32)(int addr);
	write32 = PTR_WRITE32;
	read32 = PTR_READ32;

	#if ARM7_DEBUG_CORE
		if(((insn>>16)&0xf)==15 && (insn & 0x200000))
			LOG(("%08x: Illegal use of R15 as base for write back value!\n",R15));
	#endif

	//Pre-Increment base address (IF POST INCREMENT - CALL BACK FUNCTION MUST DO IT)
	if(insn&0x1000000 && off)
	{
		//Up - Down bit
		if(insn&0x800000)
			rnv+=off;
		else
			rnv-=off;
	}

	// Load (LDC) data from ARM7 memory to Co-Proc memory
	if( insn&0x00100000 )
		{
			if(arm7_coproc_dt_r_callback)
				arm7_coproc_dt_r_callback(insn,prn,read32);
			else
				LOG(("%08x: Co-Processer Data Transfer executed, but no READ callback defined!\n",R15));
		}
	// Store (STC) data from Co-Proc to ARM7 memory
	else
		{
			if(arm7_coproc_dt_w_callback)
				arm7_coproc_dt_w_callback(insn,prn,write32);
			else
				LOG(("%08x: Co-Processer Data Transfer executed, but no WRITE callback defined!\n",R15));
		}

	//If writeback not used - ensure the original value of RN is restored in case co-proc callback changed value
	if((insn & 0x200000)==0)
		SET_REGISTER(rn,ornv);
}

static void HandleBranch(  data32_t insn )
{
	data32_t off = (insn & INSN_BRANCH) << 2;

	/* Save PC into LR if this is a branch with link */
	if (insn & INSN_BL)
	{
		SET_REGISTER(14,R15 + 4);
	}

	/* Sign-extend the 24-bit offset in our calculations */
	if (off & 0x2000000u)
	{
		R15 -= ((~(off | 0xfc000000u)) + 1) - 8;
	}
	else
	{
		R15 += off + 8;
	}
}

static void HandleMemSingle( data32_t insn )
{
	data32_t rn, rnv, off, rd;

	/* Fetch the offset */
	if (insn & INSN_I)
	{
		/* Register Shift */
		off = decodeShift(insn, NULL);
	}
	else
	{
		/* Immediate Value */
		off = insn & INSN_SDT_IMM;
	}

	/* Calculate Rn, accounting for PC */
	rn = (insn & INSN_RN) >> INSN_RN_SHIFT;

	if (insn & INSN_SDT_P)
	{
		/* Pre-indexed addressing */
		if (insn & INSN_SDT_U)
		{
			rnv = (GET_REGISTER(rn) + off);
		}
		else
		{
			rnv = (GET_REGISTER(rn) - off);
		}

		if (insn & INSN_SDT_W)
		{
			SET_REGISTER(rn,rnv);

	//check writeback???
		}
		else if (rn == eR15)
		{
			rnv = rnv + 8;
		}
	}
	else
	{
		/* Post-indexed addressing */
		if (rn == eR15)
		{
			rnv = R15 + 8;
		}
		else
		{
			rnv = GET_REGISTER(rn);
		}
	}

	/* Do the transfer */
	rd = (insn & INSN_RD) >> INSN_RD_SHIFT;
	if (insn & INSN_SDT_L)
	{
		/* Load */
		if (insn & INSN_SDT_B)
		{
			SET_REGISTER(rd,(data32_t) READ8(rnv));
		}
		else
		{
			if (rd == eR15)
			{
				R15 = READ32(rnv);
				R15 -= 4;
				//LDR, PC takes 2S + 2N + 1I (5 total cycles)
				ARM7_ICOUNT -= 2;
			}
			else
			{
				SET_REGISTER(rd,READ32(rnv));
			}
		}
	}
	else
	{
		/* Store */
		if (insn & INSN_SDT_B)
		{
			#if ARM7_DEBUG_CORE
				if(rd==eR15)
					LOG(("Wrote R15 in byte mode\n"));
			#endif

			WRITE8(rnv, (data8_t) GET_REGISTER(rd) & 0xffu);
		}
		else
		{
			#if ARM7_DEBUG_CORE
				if(rd==eR15)
					LOG(("Wrote R15 in 32bit mode\n"));
			#endif

			//WRITE32(rnv, rd == eR15 ? R15 + 8 : GET_REGISTER(rd));
			WRITE32(rnv, rd == eR15 ? R15 + 8 + 4 : GET_REGISTER(rd)); //manual says STR rd = PC, +12
		}
		//Store takes only 2 N Cycles, so add + 1
		ARM7_ICOUNT += 1;
	}

	/* Do post-indexing writeback */
	if (!(insn & INSN_SDT_P)/* && (insn&INSN_SDT_W)*/)
	{
		if (insn & INSN_SDT_U)
		{
			/* Writeback is applied in pipeline, before value is read from mem,
				so writeback is effectively ignored */
			if (rd==rn) {
				SET_REGISTER(rn,GET_REGISTER(rd));
				//todo: check for offs... ?
			}
			else {

				if ((insn&INSN_SDT_W)!=0)
					LOG(("%08x:  RegisterWritebackIncrement %d %d %d\n",R15,(insn & INSN_SDT_P)!=0,(insn&INSN_SDT_W)!=0,(insn & INSN_SDT_U)!=0));

				SET_REGISTER(rn,(rnv + off));
			}
		}
		else
		{
			/* Writeback is applied in pipeline, before value is read from mem,
				so writeback is effectively ignored */
			if (rd==rn) {
				SET_REGISTER(rn,GET_REGISTER(rd));
			}
			else {
				SET_REGISTER(rn,(rnv - off));

				if ((insn&INSN_SDT_W)!=0)
				LOG(("%08x:  RegisterWritebackDecrement %d %d %d\n",R15,(insn & INSN_SDT_P)!=0,(insn&INSN_SDT_W)!=0,(insn & INSN_SDT_U)!=0));
			}
		}
	}

//	ARM7_CHECKIRQ

} /* HandleMemSingle */

static void HandleHalfWordDT(data32_t insn)
{
	data32_t rn, rnv, off, rd;

	//Immediate or Register Offset?
	if(insn & 0x400000) {				//Bit 22 - 1 = immediate, 0 = register
		//imm. value in high nibble (bits 8-11) and lo nibble (bit 0-3)
		off = (((insn>>8)&0x0f)<<4) | (insn&0x0f);
	}
    else {
		//register
		off = GET_REGISTER(insn & 0x0f);
	}

	/* Calculate Rn, accounting for PC */
	rn = (insn & INSN_RN) >> INSN_RN_SHIFT;

	if (insn & INSN_SDT_P)
	{
		/* Pre-indexed addressing */
		if (insn & INSN_SDT_U)
		{
			rnv = (GET_REGISTER(rn) + off);
		}
		else
		{
			rnv = (GET_REGISTER(rn) - off);
		}

		if (insn & INSN_SDT_W)
		{
			SET_REGISTER(rn,rnv);

		//check writeback???
		}
		else if (rn == eR15)
		{
			rnv = (rnv) + 8;
		}
	}
	else
	{
		/* Post-indexed addressing */
		if (rn == eR15)
		{
			rnv = R15 + 8;
		}
		else
		{
			rnv = GET_REGISTER(rn);
		}
	}

	/* Do the transfer */
	rd = (insn & INSN_RD) >> INSN_RD_SHIFT;

	/* Load */
	if (insn & INSN_SDT_L)
	{
		//Signed?
		if(insn & 0x40)
		{
			data32_t newval = 0;

			//Signed Half Word?
			if(insn & 0x20) {
				data16_t signbyte,databyte;
				databyte = READ16(rnv) & 0xFFFF;
				signbyte = (databyte & 0x8000) ? 0xffff : 0;
				newval = (data32_t)(signbyte<<16)|databyte;
			}
			//Signed Byte
			else {
				data8_t databyte;
				data32_t signbyte;
				databyte = READ8(rnv) & 0xff;
				signbyte = (databyte & 0x80) ? 0xffffff : 0;
				newval = (data32_t)(signbyte<<8)|databyte;
			}

			//PC?
			if(rd == eR15)
			{
				R15 = newval + 8;
				//LDR(H,SH,SB) PC takes 2S + 2N + 1I (5 total cycles)
				ARM7_ICOUNT -= 2;

			}
			else
			{
				SET_REGISTER(rd,newval);
				R15 += 4;
			}
		}
		//Unsigned Half Word
		else
		{
			if (rd == eR15)
			{
				R15 = READ16(rnv) + 8;
			}
			else
			{
				SET_REGISTER(rd,READ16(rnv));
				R15 += 4;
			}
		}
	}
	/* Store */
	else
	{
		//WRITE16(rnv, rd == eR15 ? R15 + 8 : GET_REGISTER(rd));
		WRITE16(rnv, rd == eR15 ? R15 + 8 + 4 : GET_REGISTER(rd)); //manual says STR RD=PC, +12 of address
		if(rn != eR15)
			R15 += 4;
		//STRH takes 2 cycles, so we add + 1
		ARM7_ICOUNT += 1;
	}

	//SJE: No idea if this writeback code works or makes sense here..

	/* Do post-indexing writeback */
	if (!(insn & INSN_SDT_P)/* && (insn&INSN_SDT_W)*/)
	{
		if (insn & INSN_SDT_U)
		{
			/* Writeback is applied in pipeline, before value is read from mem,
				so writeback is effectively ignored */
			if (rd==rn) {
				SET_REGISTER(rn,GET_REGISTER(rd));
				//todo: check for offs... ?
			}
			else {

				if ((insn&INSN_SDT_W)!=0)
					LOG(("%08x:  RegisterWritebackIncrement %d %d %d\n",R15,(insn & INSN_SDT_P)!=0,(insn&INSN_SDT_W)!=0,(insn & INSN_SDT_U)!=0));

				SET_REGISTER(rn,(rnv + off));
			}
		}
		else
		{
			/* Writeback is applied in pipeline, before value is read from mem,
				so writeback is effectively ignored */
			if (rd==rn) {
				SET_REGISTER(rn,GET_REGISTER(rd));
			}
			else {
				SET_REGISTER(rn,(rnv - off));

				if ((insn&INSN_SDT_W)!=0)
					LOG(("%08x:  RegisterWritebackDecrement %d %d %d\n",R15,(insn & INSN_SDT_P)!=0,(insn&INSN_SDT_W)!=0,(insn & INSN_SDT_U)!=0));
			}
		}
	}
}

static void HandleSwap(data32_t insn)
{
	//According to manual - swap is an LDR followed by an STR and all endian rules apply
	//Process: Read original data from address pointed by Rn then store data from address
	//         pointed by Rm to Rn address, and store original data from Rn to Rd.
	// Todo: I have no valid source to verify this function works.. so it needs to be tested
	data32_t rn,rnv,rm,rmv,rd;

	LOG(("%08x: HandleSwap called!\n",R15));

	rn = GET_REGISTER((insn>>16)&0xf);
	rm = GET_REGISTER(insn&0xf);
	rd = GET_REGISTER((insn>>12)&0xf);

	#if ARM7_DEBUG_CORE
	if(rn == 15 || rm == 15 || rd == 15)
		LOG(("%08x: Illegal use of R15 in Swap Instruction\n",R15));
	#endif

	//Byte Swap?
	if(insn & 0x400000)
	{
		rnv = READ8(rn) & 0xff;	//mask to 8 bit just in case
		rmv = READ8(rm) & 0xff;
	}
	else
	{
		rnv = READ32(rn);
		rmv = READ32(rm);
	}

	//Do the swap
	SET_REGISTER(rn,rmv);
	SET_REGISTER(rd,rnv);

	R15 += 4;
	//Instruction takes 1S+2N+1I cycles - so we subtract one more..
	ARM7_ICOUNT -=1;
}

static void HandlePSRTransfer( data32_t insn )
{
	int reg = (insn & 0x400000)?SPSR:eCPSR;	//Either CPSR or SPSR
	int val = 0;
	int oldmode = GET_CPSR & MODE_FLAG;

	//MSR ( bit 21 set ) - Copy value to CPSR/SPSR
	if( (insn & 0x00200000) ) {

		//MSR (register transfer)?
		if(insn & 0x10000)
		{
			val = GET_REGISTER(insn & 0x0f);

			//If in non-privelge mode -> only condition codes (top 4 bits) can be changed!
			if( (GET_MODE==eARM7_MODE_USER))
				val = (val & 0xF0000000);
		}
		//MSR (register or immediate transfer - flag bits only)
		else
		{
			//Immediate Value?
			if(insn & INSN_I) {
				//Value can be specified for a Right Rotate, 2x the value specified.
				int by = (insn & INSN_OP2_ROTATE) >> INSN_OP2_ROTATE_SHIFT;
				if (by)
					val = ROR(insn & INSN_OP2_IMM, by << 1);
				else
					val = insn & INSN_OP2_IMM;
			}
			//Value from Register
			else {
				val = GET_REGISTER(insn & 0x0f);
			}

			//This instruction is flag bits only, so keep only top 4 bits!
			val = (val & 0xF0000000);
		}

		//Update the Register
		SET_REGISTER(reg,val);

		//Switch to new mode if changed
		if( (val & MODE_FLAG) != oldmode)
			SwitchMode(GET_MODE);

	}
	//MRS ( bit 21 clear ) - Copy CPSR or SPSR to specified Register
	else {
		SET_REGISTER( (insn>>12)& 0x0f ,GET_REGISTER(reg));
	}
}

static void HandleALU( data32_t insn )
{
	data32_t op2, sc=0, rd, rn, opcode;
	data32_t by, rdn;

	opcode = (insn & INSN_OPCODE) >> INSN_OPCODE_SHIFT;

	rd = 0;
	rn = 0;

	/* --------------*/
	/* Construct Op2 */
	/* --------------*/

	/* Immediate constant */
	if (insn & INSN_I)
	{
		by = (insn & INSN_OP2_ROTATE) >> INSN_OP2_ROTATE_SHIFT;
		if (by)
		{
			op2 = ROR(insn & INSN_OP2_IMM, by << 1);
			sc = op2 & SIGN_BIT;
		}
		else
		{
			op2 = insn & INSN_OP2_IMM;
			sc = GET_CPSR & C_MASK;
		}
	}
	/* Op2 = Register Value */
	else
	{
		op2 = decodeShift(insn, (insn & INSN_S && (opcode & 4) == 4)? &sc : NULL);

		if (!(insn & INSN_S && (opcode & 4) == 4))
			sc=0;
	}

	/* Calculate Rn to account for pipelining */
	if ((opcode & 0xd) != 0xd) /* No Rn in MOV */
	{
		if ((rn = (insn & INSN_RN) >> INSN_RN_SHIFT) == eR15)
		{
			int addpc = (insn&INSN_I?8:insn&0x10u?12:8);
			#if ARM7_DEBUG_CORE
				LOG(("%08x:  Pipelined R15 (Shift %d)\n",R15,addpc));
			#endif
			//rn=R15+8;
			rn=R15+addpc;
		}
		else
		{
			rn = GET_REGISTER(rn);
		}
	}

	/* Perform the operation */
			
	switch (opcode)
	{
	/* Arithmetic operations */
	case OPCODE_SBC:
		rd = (rn - op2 - (GET_CPSR & C_MASK ? 0 : 1));
		HandleALUSubFlags(rd, rn, op2);
		break;
	case OPCODE_CMP:
	case OPCODE_SUB:
		rd = (rn - op2);
		HandleALUSubFlags(rd, rn, op2);
		break;
	case OPCODE_RSC:
		rd = (op2 - rn - (GET_CPSR & C_MASK ? 0 : 1));
		HandleALUSubFlags(rd, op2, rn);
		break;
	case OPCODE_RSB:
		rd = (op2 - rn);
		HandleALUSubFlags(rd, op2, rn);
		break;
	case OPCODE_ADC:
		rd = (rn + op2 + ((GET_CPSR & C_MASK) >> C_BIT));
		HandleALUAddFlags(rd, rn, op2);
		break;
	case OPCODE_CMN:
	case OPCODE_ADD:
		rd = (rn + op2);
		HandleALUAddFlags(rd, rn, op2);
		break;

	/* Logical operations */
	case OPCODE_AND:
	case OPCODE_TST:
		rd = rn & op2;
		HandleALULogicalFlags(rd, sc);
		break;
	case OPCODE_BIC:
		rd = rn &~ op2;
		HandleALULogicalFlags(rd, sc);
		break;
	case OPCODE_TEQ:
	case OPCODE_EOR:
		rd = rn ^ op2;
		HandleALULogicalFlags(rd, sc);
		break;
	case OPCODE_ORR:
		rd = rn | op2;
		HandleALULogicalFlags(rd, sc);
		break;
	case OPCODE_MOV:
		rd = op2;
		HandleALULogicalFlags(rd, sc);
		break;
	case OPCODE_MVN:
		rd = (~op2);
		HandleALULogicalFlags(rd, sc);
		break;
	}

	/* Put the result in its register if not one of the test only opcodes (TST,TEQ,CMP,CMN) */
	rdn = (insn & INSN_RD) >> INSN_RD_SHIFT;
	if ((opcode & 0xc) != 0x8)
	{
		//If Rd = R15, but S Flag not set, Result is placed in R15, but CPSR is not affected (page 44)
		if (rdn == eR15 && !(insn & INSN_S))
		{
			R15 = rd;
		}
		else
		{
			//Rd = 15 and S Flag IS set, Result is placed in R15, and current mode SPSR moved to CPSR
			if (rdn==eR15) {

				//Update CPSR from SPSR
				SET_CPSR(GET_REGISTER(SPSR));
				SwitchMode(GET_MODE);

				R15 = rd;

				/* IRQ masks may have changed in this instruction */
//				ARM7_CHECKIRQ;
			}
			else
				/* S Flag is set - Write results to register & update CPSR (which was already handled using HandleALU flag macros) */
				SET_REGISTER(rdn,rd);
		}
	} 
	//SJE: Don't think this applies any more.. (see page 44 at bottom)
	/* TST & TEQ can affect R15 (the condition code register) with the S bit set */
	else if (rdn==eR15)
	{
		if (insn & INSN_S) {
			#if ARM7_DEBUG_CORE
				LOG(("%08x: TST class on R15 s bit set\n",R15));
			#endif
			R15 = rd;

			/* IRQ masks may have changed in this instruction */
//			ARM7_CHECKIRQ;
		}
		else
		{
			#if	ARM7_DEBUG_CORE
				LOG(("%08x: TST class on R15 no s bit set\n",R15));
			#endif
		}
	}
}

static void HandleMul( data32_t insn)
{
	data32_t r;

	/* Do the basic multiply of Rm and Rs */
	r =	GET_REGISTER( insn&INSN_MUL_RM ) *
	  	GET_REGISTER( (insn&INSN_MUL_RS)>>INSN_MUL_RS_SHIFT );

	#if ARM7_DEBUG_CORE
	if( 
	    ((insn&INSN_MUL_RM)==0xf) || 
		(((insn&INSN_MUL_RS)>>INSN_MUL_RS_SHIFT )==0xf) ||
		(((insn&INSN_MUL_RN)>>INSN_MUL_RN_SHIFT)==0xf)
	   )
		LOG(("%08x:  R15 used in mult\n",R15));
	#endif

	/* Add on Rn if this is a MLA */
	if (insn & INSN_MUL_A)
	{
		r += GET_REGISTER((insn&INSN_MUL_RN)>>INSN_MUL_RN_SHIFT);
	}

	/* Write the result */
	SET_REGISTER((insn&INSN_MUL_RD)>>INSN_MUL_RD_SHIFT,r);

	/* Set N and Z if asked */
	if( insn & INSN_S )
	{
		SET_CPSR ( (GET_CPSR &~ (N_MASK | Z_MASK)) | HandleALUNZFlags(r));
	}
}

//todo: add proper cycle counts
static void HandleSMulLong( data32_t insn)
{
	INT32 rm, rs;
	data32_t rhi,rlo;
	INT64 res=0;

	rm  = (INT32)GET_REGISTER(insn&0xf);
	rs  = (INT32)GET_REGISTER(((insn>>8)&0xf));
	rhi = (insn>>16)&0xf;
	rlo = (insn>>12)&0xf;

	#if ARM7_DEBUG_CORE
		if( ((insn&0xf) == 15) || (((insn>>8)&0xf) == 15) || (((insn>>16)&0xf) == 15) || (((insn>>12)&0xf) == 15) )
			LOG(("%08x: Illegal use of PC as a register in SMULL opcode\n",R15));
	#endif

	/* Perform the multiplication */
	res = (INT64)rm * rs;

	/* Add on Rn if this is a MLA */
	if (insn & INSN_MUL_A)
	{
		INT64 acum = (INT64)((((INT64)(rhi))<<32) | rlo);
		res += acum;
	}

	/* Write the result (upper dword goes to RHi, lower to RLo) */
	SET_REGISTER(rhi, res>>32);
	SET_REGISTER(rlo, res & 0xFFFFFFFF);
	
	/* Set N and Z if asked */
	if( insn & INSN_S )
	{
		SET_CPSR ( (GET_CPSR &~ (N_MASK | Z_MASK)) | HandleLongALUNZFlags(res));
	}
}

//todo: add proper cycle counts
static void HandleUMulLong( data32_t insn)
{
	UINT32 rm, rs;
	data32_t rhi,rlo;
	UINT64 res=0;

	rm  = (UINT32)GET_REGISTER(insn&0xf);
	rs  = (UINT32)GET_REGISTER(((insn>>8)&0xf));
	rhi = (insn>>16)&0xf;
	rlo = (insn>>12)&0xf;

	#if ARM7_DEBUG_CORE
		if( ((insn&0xf) == 15) || (((insn>>8)&0xf) == 15) || (((insn>>16)&0xf) == 15) || (((insn>>12)&0xf) == 15) )
			LOG(("%08x: Illegal use of PC as a register in SMULL opcode\n",R15));
	#endif

	/* Perform the multiplication */
	res = (UINT64)rm * rs;

	/* Add on Rn if this is a MLA */
	if (insn & INSN_MUL_A)
	{
		UINT64 acum = (UINT64)((((UINT64)(rhi))<<32) | rlo);
		res += acum;
	}

	/* Write the result (upper dword goes to RHi, lower to RLo) */
	SET_REGISTER(rhi, res>>32);
	SET_REGISTER(rlo, res & 0xFFFFFFFF);
	
	/* Set N and Z if asked */
	if( insn & INSN_S )
	{
		SET_CPSR ( (GET_CPSR &~ (N_MASK | Z_MASK)) | HandleLongALUNZFlags(res));
	}
}

static void HandleMemBlock( data32_t insn)
{
	data32_t rb = (insn & INSN_RN) >> INSN_RN_SHIFT;
	data32_t rbp = GET_REGISTER(rb);
	int result;

#if ARM7_DEBUG_CORE
	if(rbp & 3)
		LOG(("%08x: Unaligned Mem Transfer @ %08x\n",R15,rbp));
#endif

	//We will specify the cycle count for each case, so remove the -3 that occurs at the end
	ARM7_ICOUNT +=3;

	if (insn & INSN_BDT_L)
	{
		/* Loading */
		if (insn & INSN_BDT_U)
		{
			/* Incrementing */
			if (!(insn & INSN_BDT_P))
			{
				rbp = rbp + (- 4);
			}

			//S Flag Set, but R15 not in list = User Bank Transfer
			if(insn & INSN_BDT_S && ((insn & 0x8000)==0))
			{
				//set to user mode - then do the transfer, and set back
				int curmode = GET_MODE;
				SwitchMode(eARM7_MODE_USER);
				LOG(("%08x: User Bank Transfer not fully tested - please check if working properly!\n",R15));
				result = loadInc( insn & 0xffff, rbp, insn&INSN_BDT_S );
				//todo - not sure if Writeback occurs on User registers also.. 
				SwitchMode(curmode);
			}
			else
				result = loadInc( insn & 0xffff, rbp, insn&INSN_BDT_S );

			if (insn & INSN_BDT_W)
			{
				#if ARM7_DEBUG_CORE
					if(rb==15)
						LOG(("%08x:  Illegal LDRM writeback to r15\n",R15));
				#endif
				SET_REGISTER(rb,GET_REGISTER(rb)+result*4);
			}

			//R15 included? (NOTE: CPSR restore must occur LAST otherwise wrong registers restored!)
			if (insn & 0x8000) {
				R15-=4;		//SJE: Remove 4, since we're adding +4 after this code executes
				//S - Flag Set? Signals transfer of current mode SPSR->CPSR
				if(insn & INSN_BDT_S) {
					SET_CPSR(GET_REGISTER(SPSR));
					SwitchMode(GET_MODE);
				}
			}
			//LDM PC - takes 1 extra cycle
			ARM7_ICOUNT -=1;
		}
		else
		{
			/* Decrementing */
			if (!(insn & INSN_BDT_P))
			{
				rbp = rbp - (- 4);
			}

			//S Flag Set, but R15 not in list = User Bank Transfer
			if(insn & INSN_BDT_S && ((insn & 0x8000)==0))
			{
				//set to user mode - then do the transfer, and set back
				int curmode = GET_MODE;
				SwitchMode(eARM7_MODE_USER);
				LOG(("%08x: User Bank Transfer not fully tested - please check if working properly!\n",R15));
				result = loadDec( insn&0xffff, rbp, insn&INSN_BDT_S );
				//todo - not sure if Writeback occurs on User registers also.. 
				SwitchMode(curmode);
			}
			else
				result = loadDec( insn&0xffff, rbp, insn&INSN_BDT_S );

			if (insn & INSN_BDT_W)
			{
				if (rb==0xf)
					LOG(("%08x:  Illegal LDRM writeback to r15\n",R15));
				SET_REGISTER(rb,GET_REGISTER(rb)-result*4);
			}
			
			//R15 included? (NOTE: CPSR restore must occur LAST otherwise wrong registers restored!)
			if (insn & 0x8000) {
				R15-=4;		//SJE: Remove 4, since we're adding +4 after this code executes
				//S - Flag Set? Signals transfer of current mode SPSR->CPSR
				if(insn & INSN_BDT_S) {
					SET_CPSR(GET_REGISTER(SPSR));
					SwitchMode(GET_MODE);
				}
				//LDM PC - takes 1 extra cycle
				ARM7_ICOUNT -=1;
			}

			//LDM (NO PC) takes nS + 1n + 1I cycles (n = # of register transfers)
			ARM7_ICOUNT -= (result+1+1);
		}
	} /* Loading */
	else
	{
		/* Storing */
		if (insn & (1<<eR15))
		{
			#if ARM7_DEBUG_CORE
				LOG(("%08x: Writing R15 in strm\n",R15));
			#endif
			/* special case handling if writing to PC */
			R15 += 12;
		}
		if (insn & INSN_BDT_U)
		{
			/* Incrementing */
			if (!(insn & INSN_BDT_P))
			{
				rbp = rbp + (- 4);
			}

			//S Flag Set, but R15 not in list = User Bank Transfer
			if(insn & INSN_BDT_S && ((insn & 0x8000)==0))
			{
				//todo: needs to be tested..
				
				//set to user mode - then do the transfer, and set back
				int curmode = GET_MODE;
				SwitchMode(eARM7_MODE_USER);
				LOG(("%08x: User Bank Transfer not fully tested - please check if working properly!\n",R15));
				result = storeInc( insn&0xffff, rbp );
				//todo - not sure if Writeback occurs on User registers also.. 
				SwitchMode(curmode);
			}
			else
				result = storeInc( insn&0xffff, rbp );

			if( insn & INSN_BDT_W )
			{
				SET_REGISTER(rb,GET_REGISTER(rb)+result*4);
			}
		}
		else
		{
			/* Decrementing */
			if (!(insn & INSN_BDT_P))
			{
				rbp = rbp - (- 4);
			}

			//S Flag Set, but R15 not in list = User Bank Transfer
			if(insn & INSN_BDT_S && ((insn & 0x8000)==0))
			{
				//set to user mode - then do the transfer, and set back
				int curmode = GET_MODE;
				SwitchMode(eARM7_MODE_USER);
				LOG(("%08x: User Bank Transfer not fully tested - please check if working properly!\n",R15));
				result = storeDec( insn&0xffff, rbp );
				//todo - not sure if Writeback occurs on User registers also.. 
				SwitchMode(curmode);
			}
			else
				result = storeDec( insn&0xffff, rbp );

			if( insn & INSN_BDT_W )
			{
				SET_REGISTER(rb,GET_REGISTER(rb)-result*4);
			}
		}
		if( insn & (1<<eR15) )
			R15 -= 12; //SJE: We added 12 for storing, but put it back as it was for executing

		//STM takes (n+1)S+2N+1I cycles (n = # of register transfers)
		ARM7_ICOUNT -= ((result+1)+2+1);
	}
} /* HandleMemBlock */


