inferno-os/os/pxa/sa1110break.c

#include "u.h"
#include "../port/lib.h"
#include "mem.h"
#include "dat.h"
#include "fns.h"
#include "ureg.h"
#include "../port/error.h"

//
// from trap.c
//
extern int (*breakhandler)(Ureg *ur, Proc*);
extern Instr BREAK;
extern void portbreakinit(void);

//
// Instructions that can have the PC as a destination register
//
enum {
	IADD = 1,
	IBRANCH,
	ILDM,
	ILDR,
	IMOV,

	//
	// These should eventually be implemented
	//
	IADC,
	IAND,
	IBIC,
	IEOR,
	ILDRT,
	IMRS,
	IMVN,
	IORR,
	IRSB,
	IRSC,
	ISBC,
	ISUB,
};

static int	instrtype(Instr i);
static ulong	iadd(Ureg *ur, Instr i);
static ulong	ibranch(Ureg *ur, Instr i);
static ulong	ildm(Ureg *ur, Instr i);
static ulong	ildr(Ureg *ur, Instr i);
static ulong	imov(Ureg *ur, Instr i);
static ulong	shifterval(Ureg *ur, Instr i);
static int	condpass(Instr i, ulong psr);
static ulong	*address(Ureg *ur, Instr i);
static ulong*	multiaddr(Ureg *ur, Instr i);
static int	nbits(ulong v);

#define COND_N(psr)	(((psr) >> 31) & 1)
#define COND_Z(psr)	(((psr) >> 30) & 1)
#define COND_C(psr)	(((psr) >> 29) & 1)
#define COND_V(psr)	(((psr) >> 28) & 1)
#define REG(i, a, b)	(((i) & BITS((a), (b))) >> (a))
#define REGVAL(ur, r)	(*((ulong*)(ur) + (r)))
#define LSR(v, s)	((ulong)(v) >> (s))
#define ASR(v, s)	((long)(v) >> (s))
#define ROR(v, s)	(LSR((v), (s)) | (((v) & ((1 << (s))-1)) << (32 - (s))))

void
machbreakinit(void)
{
	portbreakinit();
	breakhandler = breakhit;
}

Instr
machinstr(ulong addr)
{
	if (addr < KTZERO)
		error(Ebadarg);
	return *(Instr*)addr;
}

void
machbreakset(ulong addr)
{
	if (addr < KTZERO)
		error(Ebadarg);
	*(Instr*)addr = BREAK;
	segflush((void*)addr, sizeof(Instr));
}

void
machbreakclear(ulong addr, Instr i)
{
	if (addr < KTZERO)
		error(Ebadarg);
	*(Instr*)addr = i;
	segflush((void*)addr, sizeof(Instr));
}

//
// Return the address of the instruction that will be executed after the
// instruction at address ur->pc.
//
// This means decoding the instruction at ur->pc.
//
// In the simple case, the PC will simply be the address of the next
// sequential instruction following ur->pc.
//
// In the complex case, the instruction is a branch of some sort, so the
// value of the PC after the instruction must be computed by decoding
// and simulating the instruction enough to determine the PC.
//

ulong
machnextaddr(Ureg *ur)
{
	Instr i;
	i = machinstr(ur->pc);
	switch(instrtype(i)) {
		case IADD:	return iadd(ur,i);
		case IBRANCH:	return ibranch(ur,i);
		case ILDM:	return ildm(ur,i);
		case ILDR:	return ildr(ur,i);
		case IMOV:	return imov(ur,i);

		case IADC:
		case IAND:
		case IBIC:
		case IEOR:
		case ILDRT:
		case IMRS:
		case IMVN:
		case IORR:
		case IRSB:
		case IRSC:
		case ISBC:
		case ISUB:
			// XXX - Tad: unimplemented
			//
			// any of these instructions could possibly have the
			// PC as Rd.  Eventually, these should all be
			// checked just like the others.
		default:
			return ur->pc+4;
	}

	return 0;
}

static int
instrtype(Instr i)
{
	if(i & BITS(26,27) == 0) {
		switch((i >> 21) & 0xF) {
			case 0:		return IAND;
			case 1:		return IEOR;
			case 2:		return ISUB;
			case 3:		return IRSB;
			case 4:		return IADD;
			case 5:		return IADC;
			case 6:		return ISBC;
			case 7:		return IRSC;
			case 0xD:	return IMOV;
			case 0xC:	return IORR;
			case 0xE:	return IBIC;
			case 0xF:	return IMVN;
		}
		if(((i & BIT(25)|BITS(23,24)|BITS(20,21))) >> 20 == 0x10)
			return IMRS;
		return 0;
	}

	if(((i & BITS(27,25)|BIT(20)) >> 20) == 0x81) return ILDM;
	if(((i & BITS(26,27)|BIT(22)|BIT(20)) >> 20) == 0x41) return ILDR;
	if(((i & BITS(25,27)) >> 25) == 5) return IBRANCH;

	return 0;
}

static ulong
iadd(Ureg *ur, Instr i)
{
	ulong Rd = REG(i, 12, 15);
	ulong Rn = REG(i, 16, 19);

	if(Rd != 15 || !condpass(i, ur->psr))
		return ur->pc+4;

	return REGVAL(ur, Rn) + shifterval(ur, i);
}

static ulong
ibranch(Ureg *ur, Instr i)
{
	if(!condpass(i, ur->psr))
		return ur->pc+4;
	return ur->pc + ((signed long)(i << 8) >> 6) + 8;
}

static ulong
ildm(Ureg *ur, Instr i)
{
	if((i & BIT(15)) == 0)
		return ur->pc+4;

	return *(multiaddr(ur, i) + nbits(i & BITS(15, 0)));
}

static ulong
ildr(Ureg *ur, Instr i)
{
	if(REG(i, 12, 19) != 15 || !condpass(i, ur->psr))
		return ur->pc+4;

	return *address(ur, i);
}

static ulong
imov(Ureg *ur, Instr i)
{
	if(REG(i, 12, 15) != 15 || !condpass(i, ur->psr))
		return ur->pc+4;

	return shifterval(ur, i);
}

static int
condpass(Instr i, ulong psr)
{
	uchar n = COND_N(psr);
	uchar z = COND_Z(psr);
	uchar c = COND_C(psr);
	uchar v = COND_V(psr);

	switch(LSR(i,28)) {
		case 0:		return z;
		case 1:		return !z;
		case 2:		return c;
		case 3:		return !c;
		case 4:		return n;
		case 5:		return !n;
		case 6:		return v;
		case 7:		return !v;
		case 8:		return c && !z;
		case 9:		return !c || z;
		case 10:	return n == v;
		case 11:	return n != v;
		case 12:	return !z && (n == v);
		case 13:	return z && (n != v);
		case 14:	return 1;
		case 15:	return 0;
	}
}

static ulong
shifterval(Ureg *ur, Instr i)
{
	if(i & BIT(25)) {					// IMMEDIATE
		ulong imm = i & BITS(0,7);
		ulong s = (i & BITS(8,11)) >> 7;  // this contains the * 2
		return ROR(imm, s);
	} else {
		ulong Rm = REGVAL(ur, REG(i, 0, 3));
		ulong s = (i & BITS(7,11)) >> 7;

		switch((i & BITS(6,4)) >> 4) {
		case 0: 					// LSL
			return Rm << s;
		case 1:						// LSLREG
			s = REGVAL(ur, s >> 1) & 0xFF;
			if(s >= 32) return 0;
			return Rm << s;
		case 2: 					// LSRIMM
			return LSR(Rm, s);
		case 3: 					// LSRREG
			s = REGVAL(ur, s >> 1) & 0xFF;
			if(s >= 32) return 0;
			return LSR(Rm, s);
		case 4:						// ASRIMM
			if(s == 0) {
				if(Rm & BIT(31) == 0)
					return 0;
				return 0xFFFFFFFF;
			}
			return ASR(Rm, s);
		case 5:						// ASRREG
			s = REGVAL(ur, s >> 1) & 0xFF;
			if(s >= 32) {
				if(Rm & BIT(31) == 0)
					return 0;
				return 0xFFFFFFFF;
			}
			return ASR(Rm, s);
		case 6: 					// RORIMM
			if(s == 0)
				return (COND_C(ur->psr) << 31) | LSR(Rm, 1);
			return ROR(Rm, s);
		case 7: 					// RORREG
			s = REGVAL(ur, s >> 1) & 0xFF;
			if(s == 0 || (s & 0xF) == 0)
				return Rm;
			return ROR(Rm, s & 0xF);
		}
	}
}

static ulong*
address(Ureg *ur, Instr i)
{
	ulong Rn = REGVAL(ur, REG(i, 16, 19));

	if(i & BIT(24) == 0) 					// POSTIDX
		return (ulong*)REGVAL(ur, Rn);
	if(i & BIT(25) == 0) {					// OFFSET
		if(i & BIT(23))
			return (ulong*)(REGVAL(ur, Rn) + (i & BITS(0, 11)));
		return (ulong*)(REGVAL(ur, Rn) - (i & BITS(0, 11)));
	} else {						// REGOFF
		ulong Rm = REGVAL(ur, REG(i, 0, 3));
		ulong index = 0;
		switch(i & BITS(5,6) >> 5) {
		case 0:	index = Rm << ((i & BITS(7, 11)) >> 7);		break;
		case 1:	index = LSR(Rm, ((i & BITS(7, 11)) >> 7));	break;
		case 2:	index = ASR(Rm, ((i & BITS(7, 11)) >> 7));	break;
		case 3:
			if(i & BITS(7, 11) == 0)
				index = (COND_C(ur->psr) << 31) | LSR(Rm, 1);
			else
				index = ROR(Rm, (i & BITS(7, 11)) >> 7);
			break;
		}
		if(i & BIT(23))
			return (ulong*)(Rn + index);
		return (ulong*)(Rn - index);
	}
}

static ulong*
multiaddr(Ureg *ur, Instr i)
{
	ulong Rn = REGVAL(ur, REG(i, 16, 19));

	switch((i >> 23) & 3) {
		case 0: return (ulong*)(Rn - (nbits(i & BITS(0,15))*4)+4);
		case 1:	return (ulong*)Rn;
		case 2: return (ulong*)(Rn - (nbits(i & BITS(0,15))*4));
		case 3: return (ulong*)(Rn + 4);
	}
}

static int
nbits(ulong v)
{
	int n = 0;
	int i;
	for(i = 0; i < 32; i++) {
		if(v & 1)
			++n;
		v = LSR(v, 1);
	}
	return n;
}