/* Copyright (c)1994-2000 Begemot Computer Associates. All rights reserved.
 * See the file COPYRIGHT for details of redistribution and use. */

# include "regi.h"
# include "proc.h"

RCSID("$Id: fpp.c 510 2006-10-26 14:18:49Z brandt_h $")

# define DEF(N) void N(void); void N(void)


# ifdef FPP

struct ifloat;

# define DX(C)	C
# define D(C)	printf C
# define DD(FL)	printf("F="), dmpflt(&(FL)), printf(" %g\n", mkd(&(FL)));
# define M(M)	printf("m=%08x%08x\n", ((u_int *)&M)[0], ((u_int *)&M)[1])

void 		unfold(pdp_float_t *, struct ifloat *, int);
int 		fold(struct ifloat *, pdp_float_t *, int, int);
static double	mkdouble(pdp_float_t *f, int);
static double	mkd(struct ifloat *);
static void	dmpflt(struct ifloat *) UNUSED;
static void 	fpp_trap(int err, int bit);
static void	do_dst(struct ifloat *, pdp_float_t *);
static void	do_dst_traps(pdp_float_t *, int);


# define FCC	proc.fpp.st &= ~FC
# define FCV	proc.fpp.st &= ~FV 
# define FCZ	proc.fpp.st &= ~FZ
# define FCN	proc.fpp.st &= ~FN

# define FSC 	proc.fpp.st |= FC
# define FSV	proc.fpp.st |= FV
# define FSZ	proc.fpp.st |= FZ
# define FSN	proc.fpp.st |= FN


/*
 * FPP
 * fetching and storing is done 'by hand' rather than trough
 * 'microcode', because this would require a lot of fetch/store
 * microcode routines and I really don't need a speedy floating point
 */

static pdp_float_t pdp_null;
static pdp_float_t fsrc;	/* other operand is register */
static int	   fsrc_undef;	/* if operand loaded from mem and -0 */

# include "float.h"

/*
 * Trap handling
 * we can load FEA from proc.mmr2 here because proc.mmr2 is not the real
 * MMR2 but holds the address of the last instruction in ANY case. Even
 * if the MMU is off or is frozen. If the MMU is off, proc.ummr2 holds
 * the last mapped mmr2 contents. If the MMU is on but frozen proc.fmmr2
 * holds the frozen contents.
 */
static inline int
will_trap(int err, int bit)
{
	if(bit == 0 || (proc.fpp.st & bit)) {
		proc.fpp.fea = proc.mmr2;
		proc.fpp.fec = err;
		proc.fpp.st |= FER;
	}
	return !(proc.fpp.st & FID) && (bit == 0 || (proc.fpp.st & bit));
}

static inline void
fpp_trap(int err, int bit)
{
	if(will_trap(err, bit))
		Trap(0244);
}


/*
 * check if we have the undefined variable
 */
static inline int 
undef(pdp_float_t *f)
{
	return f->e == 0 && f->s;
}

/*
 * check for null (+0 or -0)
 */
static inline int
null(pdp_float_t *f)
{
	return f->e == 0;
}


/*
 * if src is undef and interrupt on undef enabled then trap
 */
static inline void
check_undef_src(void)
{
	if(fsrc_undef)
		fpp_trap(FE_UND, FIUV);
}


DEF(CFCC) {
	proc.n = ((proc.fpp.st & FN) != 0);
	proc.z = ((proc.fpp.st & FZ) != 0);
	proc.v = ((proc.fpp.st & FV) != 0);
	proc.c = ((proc.fpp.st & FC) != 0);
}

DEF(SETF) { proc.fpp.st &= ~FD; }
DEF(SETI) { proc.fpp.st &= ~FL; }
DEF(SETD) { proc.fpp.st |= FD; }
DEF(SETL) { proc.fpp.st |= FL; }

DEF(LDFPS) {
	src &= FER|FID|FIUV|FIU|FIV|FIC|FD|FL|FT|FN|FZ|FV|FC;
	proc.fpp.st = src;
}

DEF(STFPS) {
	dst = proc.fpp.st;
}

DEF(STST) {
	src = proc.fpp.fec;	/* low word */
	dst = proc.fpp.fea;	/* high word */
}

DEF(CLRF) {
	fsrc = pdp_null;
	FCC; FCV; FSZ; FCN;
}

/*
 * test float
 * trap on -0 before execution
 * -0 sets N if no trap!
 */
DEF(TSTF) {
	check_undef_src();
	if(null(&fsrc)) FSZ; else FCZ;
	if(fsrc.s) 	FSN; else FCN;
	FCC; FCV;
}

/* 
 * make absolute value
 * trap before execution on undefined operand
 */
DEF(ABSF) {
	check_undef_src();
	FCC; FCV; FCN;
	if(null(&fsrc)) {
		fsrc = pdp_null;
		FSZ;
	} else {
		FCZ;
		fsrc.s = 0;
	}
}


/*
 * negate
 * trap on -0 before
 */
DEF(NEGF) {
	check_undef_src();
	if(null(&fsrc)) {
		FCN; FSZ; fsrc = pdp_null;
	} else if(fsrc.s) {
		FCN; FCZ; fsrc.s = 0;
	} else {
		FSN; FCZ; fsrc.s = 1;
	}
	FCC; FCV;
}

/*
 * multiply
 * trap on -0 before
 */
DEF(MULF) {
	struct ifloat a, s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	check_undef_src();

	unfold(&fsrc, &s, proc.fpp.st & FD);
	unfold(ac, &a, proc.fpp.st & FD);

	InitFp(); MulF(a, s); EndFp();

	do_dst(&a, ac);
}


/*
 * mod
 */
DEF(MODF) {
	struct ifloat a, s, f, i;
	pdp_float_t *ac0 = &proc.fpp.ac[(proc.cmd >> 6) & 3];
	pdp_float_t *ac1 = &proc.fpp.ac[((proc.cmd >> 6) & 3) | 1];
	pdp_float_t prod, pi, pf;
	int condp;
	int exp_limit = (proc.fpp.st & FD) ? 56 : 24;

	check_undef_src();

	unfold(&fsrc, &s, proc.fpp.st & FD);
	unfold(ac0, &a, proc.fpp.st & FD);

# if 0
	printf("a="); dmpflt(&a); printf("   ");
	printf("s="); dmpflt(&s); printf("\n");
# endif

	InitFp(); MulF(a,s); ModF(a,f,i); EndFp();

# if 0
	printf("p="); dmpflt(&a); printf("\n");
	printf("f="); dmpflt(&f); printf("   ");
	printf("i="); dmpflt(&i); printf("\n\n");
# endif

	condp = fold(&a, &prod, 1, 1);
	fold(&i, &pi, proc.fpp.st & FD, proc.fpp.st & FT);
	fold(&f, &pf, proc.fpp.st & FD, proc.fpp.st & FT);

	FCC; FCV;
	if(pf.s) FSN; else FCN;
	if(pf.e) FCZ; else FSZ;

	if(condp & FIV) {
		/*
		 * case 1: prod overflows
		 */
		FSV;
		if(will_trap(FE_OFL, FIV)) {
			*ac1 = pi;
			*ac0 = pdp_null; FSZ; FCN;
			fpp_trap(FE_OFL, FIV);
		} else {
			*ac1 = pdp_null;
			*ac0 = pdp_null; FSZ; FCN;
		}
	} else if(condp & FIU) {
		/*
		 * case 5: prod underflows
		 */
		if(will_trap(FE_UFL, FIU)) {
			*ac1 = pdp_null;
			*ac0 = pf;	
			fpp_trap(FE_UFL, FIU);
		} else {
			*ac1 = pdp_null; 
			*ac0 = pdp_null; FSZ; FCN;
		}
	} else if(!IsZ(a) && FrExp(a) >= exp_limit) {
		/*
		 * case 2: low-order bits get lost
 		 */
		*ac1 = pi;
		*ac0 = pdp_null; FSZ; FCN;
	} else {
		*ac1 = pi;
		*ac0 = pf;
	}
}

/*
 * add to float values
 */
DEF(ADDF) {
	struct ifloat a, s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	check_undef_src();

	unfold(&fsrc, &s, proc.fpp.st & FD);
	unfold(ac, &a, proc.fpp.st & FD);

	/* DD(a); DD(s); */
	InitFp(); AddF(a, s); EndFp();
	/* DD(a); */

	do_dst(&a, ac);
}

/*
 * load float in acN
 * trap before load on -0
 */
DEF(LDF) {
	check_undef_src();
	proc.fpp.ac[(proc.cmd >> 6) & 3] = fsrc;
	if(fsrc.s) 	FSN; else FCN;
	if(null(&fsrc))	FSZ; else FCZ;
	FCC; FCV;
}

/*
 * subtract
 * trap on -0 before
 */
DEF(SUBF) {
	struct ifloat a, s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	check_undef_src();

	fsrc.s = !fsrc.s;	/* (unfold makes 0 from -0) */

	unfold(&fsrc, &s, proc.fpp.st & FD);
	unfold(ac, &a, proc.fpp.st & FD);

	InitFp(); AddF(a, s); EndFp();

	do_dst(&a, ac);
}

/*
 * compare ac and src
 *
 * trap on -0 before execution
 * any 0 is treated as 0
 * if both operands are a 0, ac is set to exact 0
 */
DEF(CMPF) {
	struct ifloat	a, s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	check_undef_src();
	FCV; FCC;

	if(null(&fsrc) && null(ac)) {
		*ac = pdp_null;
		FSZ; FCN;
		return;
	} 

	unfold(&fsrc, &s, proc.fpp.st & FD);
	unfold(ac, &a, proc.fpp.st & FD);

	InitFp();
	SubF(s, a); 
	if(IsZ(s)) FSZ; else FCZ;
	if(IsN(s)) FSN; else FCN;
	EndFp();
}

/*
 * store float/double from ac to memory
 * this does not change the floating condition codes
 * and permits storage of -0
 */
DEF(STF) {
	fsrc = proc.fpp.ac[(proc.cmd >> 6) & 3];
}


/*
 * div ac by src
 * -0 in src traps BEFORE operation
 * all dirty 0s and -0s are interpreted as 0
 * src==0 traps with er=4
 * over- and underflow can occure
 */
DEF(DIVF) {
	struct ifloat	a, s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	check_undef_src();
	if(null(&fsrc))
		fpp_trap(FE_DIV0, 0);	/* never returns */
	FCC;
	if(null(ac)) {
		*ac = pdp_null;
		FSZ; FCN; FCV;
		return;
	}

	unfold(&fsrc, &s, proc.fpp.st & FD);
	unfold(ac, &a, proc.fpp.st & FD);

	/* DD(a); DD(s); */
	InitFp();
	DivF(a, s); 		/* should produce no overflow */
	EndFp();
	/* DD(a); */

	do_dst(&a, ac);
}


/*
 * convert exponent from excess notation and store
 * note exp=0 give dst=-200!
 */
DEF(STEXP) {
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	dst = (signed short)ac->e - PDP_OFF;
	if(dst == 0) {
		FSZ; proc.z = 1;
	} else {
		FCZ; proc.z = 0;
	}
	if((signed short)dst <  0) {
		FSN; proc.n = 1;
	} else {
		FCN; proc.n = 0;
	}
	FCC; proc.c = 0;
	FCV; proc.v = 0;
}

/*
 * convert and store d/f->i/l
 * trap is number is too big
 */
DEF(STCFI) {
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];
	long res = 0;
	int err = 0;

	if(ac->e > PDP_OFF) {
		int e = ac->e - PDP_OFF;
		int l = (proc.fpp.st & FL) ? 31 : 15;

		/* 
		 * number is >= 2^(e-1), < 2^e and e > 0
		 */
		if(e <= l) {
			pdp_float_t f = *ac;
			struct ifloat s;

			f.s = 0;
			unfold(&f, &s, proc.fpp.st & FD);
			InitFp();
			CnvF2L(s, res);
			EndFp();

			if(ac->s)
				res = -res;
		} else
			err++;
	}

	proc.c = 0; FCC;
	proc.v = 0; FCV;
	proc.n = 0; FCN;
	proc.z = 0; FCZ;
	if(res < 0) {
		proc.n = 1; FSN;
	}
	if(res == 0) {
		proc.z = 1; FSZ;
	}
	if(err) {
		proc.c = 1; FSC;
	}

	
	if(proc.fpp.st & FL) {
		src = (u_short)(res >> 16);
		dst = (u_short)res;
		store_long(1);
	} else {
		src = (u_short)res;
 		store_long(0);
	}

	if(err)
		fpp_trap(FE_INT, FIC);
}

/*
 * store and convert f->d or d->f
 * overflow on rounding d->f gives +/-0
 * the traps occure only AFTER storing the result
 */
DEF(STCFD) {
	int	t;
	struct ifloat	s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	FCC;
	unfold(ac, &s, proc.fpp.st & FD);
	t = fold(&s, &fsrc, !(proc.fpp.st & FD), proc.fpp.st & FT);

	if(((t & FIU) && !will_trap(FE_UFL, FIU)) || ((t & FIV) && !will_trap(FE_OFL, FIV)))
		fsrc = pdp_null;
	store_float((u_short *)&fsrc, !(proc.fpp.st & FD), 1);
	do_dst_traps(&fsrc, t);
}

/*
 * load exponent
 * new exponent is in src
 * traps on over or underflow
 */
DEF(LDEXP) {
	int e = (signed short)src;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];
	int ret = 0;

	FCC;
	/*
	 * limit exponent to prevent to much overflow
	 */
	if(e > 0377)
		e = e % 0400;
	else if(e < -0377)
		e = -(-e % 0400);
	if(e > 0177)
		ret |= FIV;
	else if(e < -0177)
		ret |= FIU;
	ac->e = e + PDP_OFF;

	do_dst_traps(ac, ret);
}

/*
 * load and convert 16 or 32 bit to d or f
 * no exceptions
 * src is int or high; dst is low
 */
DEF(LDCIF) {
	int	v;
	struct ifloat	s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	FCC;
	if(proc.fpp.st & FL)
 		v = ((int)(signed short)src << 16) | dst;
	else
		v = (signed short)src;
	InitFp();
	CnvL(s, v);
	EndFp();

	do_dst(&s, ac);
}

/*
 * load and convert d->f or f->d
 * -0 traps before operation
 * overflow on rounding from f to d yields -0 or +0 with enabled int
 *  else exact 0
 */
DEF(LDCDF) {
	struct ifloat	s;
	pdp_float_t *ac = &proc.fpp.ac[(proc.cmd >> 6) & 3];

	check_undef_src();
	FCC;

	if(proc.fpp.st & FD)
		fsrc.m2 = fsrc.m3 = 0;
	unfold(&fsrc, &s, !(proc.fpp.st & FD));
	do_dst(&s, ac);
}


DEF(FILL) {
	if(ill_trace) {
		printf("ill fpp-inst: pc=%06o curmode=%d inst=%06o\n", proc.reg[7],
				proc.curmode, proc.cmd);
	}
	fpp_trap(FE_ILL, 0);
}
/*
 * store float into ac 
 * set N/Z/V
 * do traps on fold result
 */
static void
do_dst_traps(pdp_float_t *ac, int cond)
{
	if(ac->s) FSN; else FCN;
	if(ac->e) FCZ; else FSZ;

	FCV;
	if(cond & FIU) {
		fpp_trap(FE_UFL, FIU);
		*ac = pdp_null; FSZ;
	} else if(cond & FIV) {
		FSV;
		fpp_trap(FE_OFL, FIV);
		*ac = pdp_null; FSZ;
	}
}


static void	
do_dst(struct ifloat *f, pdp_float_t *ac)
{
	do_dst_traps(ac, fold(f, ac, proc.fpp.st & FD, proc.fpp.st & FT));
}

/*
 * trap to 10 if floating point is disabled
 */
DEF(FPP_X) {
	if(proc.fpd)
		Trap10();
}

static inline void
check_fsrc(void)
{
	if(proc.cmd & 070)
		fsrc_undef = undef(&fsrc);
	else
		fsrc_undef = 0;
}


/*
 * store for STST command
 * 
 * dst -> (dst)
 * src -> (dst + 2)
 */
DEF(FPP_A) {
	store_long(1);
}

/*
 * store float/double without preceeding fetch
 */
DEF(FPP_B) {
	store_float((u_short *)&fsrc, proc.fpp.st & FD, 1);
}

/*
 * fetch float/double without following store
 * set undef flag on memory load
 */
DEF(FPP_C) {
	load_float((u_short *)&fsrc, proc.fpp.st & FD, 1);
	check_fsrc();
}

/*
 * store float/double with preceeding fetch
 */
DEF(FPP_D) {
	store_float((u_short *)&fsrc, proc.fpp.st & FD, 0);
}

/*
 * fetch float/double with following store
 * set undef flag on memory load
 */
DEF(FPP_E) {
	load_float((u_short *)&fsrc, proc.fpp.st & FD, 0);
	check_fsrc();
}

/*
 * fetch integer/long for ldc[il][df]
 * int or high word is src
 * low word is dst
 */
DEF(FPP_Q) {
	load_long(proc.fpp.st & FL);
}

/*
 * fetch double/float for d/f conversion
 * set undef flag on memory load
 */
DEF(FPP_R) {
	load_float((u_short *)&fsrc, !(proc.fpp.st & FD), 1);
	check_fsrc();
}

/*
 * Make an float from an pdp11 float
 *
 * dirty 0 and -0 are all converted to exact 0, so all checks for them
 * must be done on the original value
 */
void
unfold(pdp_float_t *pdp, struct ifloat *h, int d)
{
# if defined(__GNUC_MINOR__) && __GNUC_MINOR__ == 96
	/* XXX fake gcc into not using the builtin memcpy. Ugh! */
	void (*func)(void *, void *, size_t) = (void (*)(void *, void *, size_t))memcpy;
	func(h, &host_null, sizeof(host_null));
# else
	*h = host_null;
# endif
	if(pdp->e != 0) {
		u_xquad_t m = ((u_xquad_t)pdp->m0 << 56) | ((u_xquad_t)pdp->m1 << 40);
		signed short e = (signed short)pdp->e - PDP_OFF;
		int s = pdp->s;

		if(d)
			m |= ((u_xquad_t)pdp->m2 << 24) | ((u_xquad_t)pdp->m3 << 8);

		/* D(("unfold M=%08x %08x E=%04x S=%d -> ", (u_long)(m >> 32), (u_long)m, e, s)); */
		SetMant(h, m);
		SetExp(h, e);
		SetSign(h, s);
		/* DX(dmpflt(h); printf("\n")); */
	}
}

/*
 * convert float back to an pdp11 float or double
 *
 * here we must check for over and underflow and round or chop
 * according to precision
 * the printf's below 'should not happen'
 * we don't handle here all these NANs, INFs or else (should not happen)
 */
int
fold(struct ifloat *h, pdp_float_t *pdp, int d, int chop)
{
	int ret = 0;
	int e;
	int s;
	u_xquad_t m;

	if(h->e == 0) {
		/*
		 * store exact 0
		 */
		pdp->e = 0;
		pdp->s = 0;
		pdp->m0 = 0;
		pdp->m1 = pdp->m2 = pdp->m3 = 0;
		return 0;
	}

	m = GetMant(h);
	e = GetExp(h);
	s = GetSign(h);

	if(!chop) {
		/*
		 * round
 		 */
		if(d) {
			m += (u_xquad_t)1 << 7;
		} else {
			m += (u_xquad_t)1 << 39;
		}
		if(m & ((u_xquad_t)0x1 << 63)) {
			e++;
			m = 0;
		}
	}

	if(e > 0177) {
		ret |= FIV;
		if(e > 0377)
			printf("fpp: (fold) double overflow: %o\n", e);
	} else if(e < -0177) {
		ret |= FIU;
		if(e < -0377)
			printf("fpp: (fold) double underflow: %o\n", e);
	}
	pdp->e = e + PDP_OFF;
	pdp->s = s ? 1 : 0;
	
	/*
	 * mantissa
	 */
	pdp->m0 = m >> 56;
	pdp->m1 = m >> 40;
	if(d) {
		pdp->m2 = m >> 24;
		pdp->m3 = m >> 8;
	} else {
		pdp->m2 = 0;
		pdp->m3 = 0;
	}
	return ret;
}


/*
 * this routine is called from monitor to print the fpp state
 */
# define DS(F) {F,#F}

static struct {
	int	f;
	char	*s;
} st_tab[] = { 
	DS(FER), 	DS(FID), 	DS(020000), 	DS(010000),
	DS(FIUV), 	DS(FIU), 	DS(FIV), 	DS(FIC),
	DS(FD), 	DS(FL), 	DS(FT), 	DS(020),
	DS(FN), 	DS(FZ), 	DS(FV), 	DS(FC),
};
# undef DS

void
mon_fpp(void)
{
	int i;
	double d;

	printf("fps=%06ho", proc.fpp.st);
	for(i = 0; i < 16; i++)
		if(proc.fpp.st & st_tab[i].f)
			printf(" %s", st_tab[i].s);
	printf("\n");
	printf("fea=%06o fec=%06o\n", proc.fpp.fea, proc.fpp.fec);

	for(i = 0; i < 6; i++) {
		d = mkdouble(&proc.fpp.ac[i], 1);
		printf("ac%d = {%06ho %06ho %06ho %06ho} = %24.16e\n", i,
			((u_short *)&proc.fpp.ac[i])[0],
			((u_short *)&proc.fpp.ac[i])[1],
			((u_short *)&proc.fpp.ac[i])[2],
			((u_short *)&proc.fpp.ac[i])[3], d);
	}

}


# else


/*
 * no FPP
 * only FPP_X is ever called and traps to 10
 */

# define FPP(N) void N(void) {}

FPP(CFCC)
FPP(SETF)
FPP(SETI)
FPP(SETD)
FPP(SETL)
FPP(LDFPS)
FPP(STFPS)
FPP(STST)
FPP(CLRF)
FPP(TSTF)
FPP(ABSF)
FPP(NEGF)
FPP(MULF)
FPP(MODF)
FPP(ADDF)
FPP(LDF)
FPP(SUBF)
FPP(CMPF)
FPP(STF)
FPP(DIVF)
FPP(STEXP)
FPP(STCFI)
FPP(STCFD)
FPP(LDEXP)
FPP(LDCIF)
FPP(LDCDF)
FPP(FILL)
FPP(FPP_A)
FPP(FPP_B)
FPP(FPP_C)
FPP(FPP_D)
FPP(FPP_E)
FPP(FPP_Q)
FPP(FPP_R)

DEF(FPP_X) {
	Trap10();
}

# endif
