ldst.cpp

RISC processor ARM-7 emulator

/*************************************************************************
    Copyright (C) 2002,2003,2004,2005 Wei Qin
    See file COPYING for more information.

    This program is free software; you can redistribute it and/or modify    
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
*************************************************************************/

#include <assert.h>
#include <armemul.h>
#include "ldst.h"

/*memory opcode fields*/
#define BFLD ((inst>>22)&1)
#define UFLD ((inst>>23)&1)
#define PFLD ((inst>>24)&1)
#define WFLD ((inst>>21)&1)
#define SIGN ((inst>>6)&1)
#define HFLD ((inst>>5)&1)
#define LFLD ((inst>>20)&1)

using namespace emulator;

/*TODO: differentiate user mode and priviledged mode*/
/*utilities for mem operations*/
static inline UInt32 rotate_right(UInt32 val, UInt32 imm)
{
	return (val >> imm ) | (val << (32 - imm));
}

/*shifter operand*/
static inline UInt32 shifter_operand(IMPL_FORMALS)
{
	UInt32 shift_imm = (inst>>7) & 0x01f, val = RM, result;
	UInt8 type = (inst>>5) & 0x03;

	switch (type) {
		case 0: /*LSL*/
			result = val << shift_imm;
			break;
		case 1: /*LSR*/
			if (shift_imm) {
				result = val >> shift_imm;
			} else {
				result = 0;
			}
			break; 
		case 2: /*ASR*/ 
			if (shift_imm) {
				result = (SInt32)val>>shift_imm;
			} 
			else {
				result = BIT31(val)?~0:BIT31(val);
			}
			break; 
		default: /*ROR*/
			if (shift_imm) {
				result = rotate_right(val, shift_imm);
			}
			else { /*RRX*/									
				result = (val>>1) | (C_FLAG<<31);			 
			}												 
			break;											
	}														 
	return result;											
}

static inline UInt32 popcount16(UInt32 x)
{
  x &= 0xFFFF;
  x = (x & 0x5555) + ((x >> 1) & 0x5555);
  x = (x & 0x3333) + ((x >> 2) & 0x3333);
  x = (x & 0x0F0F) + ((x >> 4) & 0x0F0F);
  x = (x & 0x00FF) + ((x >> 8) & 0x00FF);
  return x;
}

#if 0
void impl_ld1_imm0(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;

	if (PFLD) /*preindex*/
		address = UFLD?RN+offset:RN-offset;
	else
		address = RN;

	if (BFLD) {
		WRITE_REG(RDFLD, MEM_READ_BYTE(address));
	}
	else {
		UInt32 val = MEM_READ_WORD(address);
		WRITE_REG(RDFLD, rotate_right(val, (address&0x3)<<3));
	}

	if (!PFLD) 
		WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);
	else if (WFLD) 
		WRITE_REG(RNFLD, address);
}
#endif

/*P==0, B==0*/
void impl_ld1_imm(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address, val;

	address = RN;
	/*val = MEM_READ_WORD(address);*/
    val = MEM_READ_WORD(address);
	if (address&0x3) /*address[1:0]==0 is the common case */
		val = rotate_right(val, (address&0x3)<<3);
	WRITE_REG(RDFLD, val);
	WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);

	EMULATOR_STUB(ld1_imm,inst);
}

/*P==1, B==0*/
void impl_ld1_imm_p(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address, val;

	address = UFLD?RN+offset:RN-offset;
	/*val = MEM_READ_WORD(address);*/
    val = MEM_READ_WORD(address);
	if (address&0x3) /*address==0 is the common case */
		val = rotate_right(val, (address&0x3)<<3);
	WRITE_REG(RDFLD, val);
	if (WFLD) WRITE_REG(RNFLD, address);

	EMULATOR_STUB(ld1_imm_p,inst);
}

/*P==0, B==1*/
void impl_ld1_imm_b(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;

	address = RN;
	WRITE_REG(RDFLD, MEM_READ_BYTE(address));
	WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);

	EMULATOR_STUB(ld1_imm_b,inst);
}

/*P==1, B==1*/
void impl_ld1_imm_pb(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;

	address = UFLD?RN+offset:RN-offset;
	WRITE_REG(RDFLD, MEM_READ_BYTE(address));
	if (WFLD) WRITE_REG(RNFLD, address);

	EMULATOR_STUB(ld1_imm_pb,inst);
}

#if 0
void impl_ld1_reg(IMPL_FORMALS)
{
	UInt32 offset = shifter_operand(IMPL_ARGS);
	UInt32 address;

	if (PFLD) /*preindex*/
		address = UFLD?RN+offset:RN-offset;
	else
		address = RN;

	if (BFLD) {
		WRITE_REG(RDFLD, MEM_READ_BYTE(address));
	}
	else {
		UInt32 val = MEM_READ_WORD(address);
		if (address&0x3) /*address==0 is the common case */
			val = rotate_right(val, (address&0x3)<<3);
		WRITE_REG(RDFLD, val);
	}

	if (!PFLD) 
		WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);
	else if (WFLD) 
		WRITE_REG(RNFLD, address);
}
#endif

/*p==0, b==0*/
void impl_ld1_reg(IMPL_FORMALS)
{
	UInt32 offset = shifter_operand(IMPL_ARGS);
	UInt32 address, val;

	address = RN;

	val = MEM_READ_WORD(address);
	if (address&0x3) /*address==0 is the common case */
		val = rotate_right(val, (address&0x3)<<3);
	WRITE_REG(RDFLD, val);

	WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);

	EMULATOR_STUB(ld1_reg,inst);
}

/*p==1, b==0*/
void impl_ld1_reg_p(IMPL_FORMALS)
{
	UInt32 offset = shifter_operand(IMPL_ARGS);
	UInt32 address, val;

	address = UFLD?RN+offset:RN-offset;

	val = MEM_READ_WORD(address);
	if (address&0x3) /*address==0 is the common case */
		val = rotate_right(val, (address&0x3)<<3);
	WRITE_REG(RDFLD, val);

	if (WFLD) 
		WRITE_REG(RNFLD, address);

	EMULATOR_STUB(ld1_reg_p,inst);
}

/*p==0, b==1*/
void impl_ld1_reg_b(IMPL_FORMALS)
{
	UInt32 offset = shifter_operand(IMPL_ARGS);
	UInt32 address;

	address = RN;
	WRITE_REG(RDFLD, MEM_READ_BYTE(address));
	WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);

	EMULATOR_STUB(ld1_reg_b,inst);
}

/*p==1, b==1*/
void impl_ld1_reg_pb(IMPL_FORMALS)
{
	UInt32 offset = shifter_operand(IMPL_ARGS);
	UInt32 address;

	address = UFLD?RN+offset:RN-offset;
	WRITE_REG(RDFLD, MEM_READ_BYTE(address));
	if (WFLD) 
		WRITE_REG(RNFLD, address);

	EMULATOR_STUB(ld1_reg_pb,inst);
}

void impl_ld2_imm(IMPL_FORMALS)
{
	UInt32 offset = ((inst>>4) & 0xF0) | (inst & 0xF);
	UInt32 address;
	if (PFLD) /*preindex*/
		address = UFLD?RN+offset:RN-offset;
	else
		address = RN;

	if (!HFLD) {
		WRITE_REG(RDFLD, (SInt8)MEM_READ_BYTE(address));
	}
	else {
		UInt16 val = MEM_READ_HALF_WORD(address);
		if (address&0x1)
			WRITE_REG(RDFLD, 0xCCCCCCCC);	/*unpredictable*/
		else
			WRITE_REG(RDFLD, SIGN?(SInt16)val:val);
	}

	if (!PFLD) /*post index always updates*/
		WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);
	else if (WFLD) /*preindex and update*/
		WRITE_REG(RNFLD, address);

	EMULATOR_STUB(ld2_imm,inst);
}

void impl_ld2_reg(IMPL_FORMALS)
{
	UInt32 offset = RM;
	UInt32 address;

	if (PFLD) /*preindex*/
		address = UFLD?RN+offset:RN-offset;
	else
		address = RN;

	if (!HFLD) {
		WRITE_REG(RDFLD, (SInt8)MEM_READ_BYTE(address));
	}
	else {
		UInt16 val = MEM_READ_HALF_WORD(address);
		if (address&0x1)
			WRITE_REG(RDFLD, 0xCCCCCCCC);	/*unpredictable*/
		else
			WRITE_REG(RDFLD, SIGN?(SInt16)val:val);
	}

	if (!PFLD) 
		WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);
	else if (WFLD) 
		WRITE_REG(RNFLD, address);

	EMULATOR_STUB(ld2_reg,inst);
}

#if 0
void impl_st1_imm(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;

	if (PFLD) /*preindex*/
		address = UFLD?RN+offset:RN-offset;
	else
		address = RN;

	if (BFLD)
		MEM_WRITE_BYTE(address, RD);
	else 
		MEM_WRITE_WORD(address, RD);

	if (!PFLD) 
		WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);
	else if (WFLD) 
		WRITE_REG(RNFLD, address);
}
#endif

/*p==0, b==0*/
void impl_st1_imm(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;
	address = RN;
	/*MEM_WRITE_WORD(address, RD);*/
	MEM_WRITE_WORD(address, RD);
	WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);

	EMULATOR_STUB(st1_imm,inst);
}

/*p==1, b==0*/
void impl_st1_imm_p(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;
	address = UFLD?RN+offset:RN-offset;
	MEM_WRITE_WORD(address, RD);
	/*MEM_WRITE_WORD(address, RD);*/
	if (WFLD) 
		WRITE_REG(RNFLD, address);

	EMULATOR_STUB(st1_imm_p,inst);
}

/*p==0, b==1*/
void impl_st1_imm_b(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;
	address = RN;
	MEM_WRITE_BYTE(address, RD);
	WRITE_REG(RNFLD, UFLD?RN+offset:RN-offset);

	EMULATOR_STUB(st1_imm_b,inst);
}

/*p==1, b==1*/
void impl_st1_imm_pb(IMPL_FORMALS)
{
	UInt32 offset = inst & 0xFFF;
	UInt32 address;
	address = UFLD?RN+offset:RN-offset;
	MEM_WRITE_BYTE(address, RD);
	if (WFLD) 
		WRITE_REG(RNFLD, address);

	EMULATOR_STUB(st1_imm_pb,inst);