
#include "regs.h"
#include "s709.h"
#include "console.h"

unsigned short srchar;
unsigned short acchar;
unsigned short mqchar;
unsigned short adderchar;

/*
 * Integer arithmetic ops
 */

void
add()
{

	if (((srh & SIGN) ^ (ach & SIGN)) == 0) {
		adderl = srl + acl;
		adderh = (srh & HMSK) + ach;
		if (adderl < srl)
			adderh++;
		if ((adderh & P) != (ach & P))
			acoflo = 1;
	} else {
		adderl = srl + ~acl;
		adderh = (srh & HMSK) + (~ach & (SIGN|Q|P|HMSK));
		if (adderl < srl)
			adderh++;
		if (adderh & QCARRY) {
			adderl++;
			if (adderl == 0)
				adderh++;
		} else {
			adderl = ~adderl;
			adderh = ~adderh;
		}
	}
	acl = adderl;
	ach = adderh & (SIGN|Q|P|HMSK);
}

void
rnd()
{

	if (mqh & 04) {
		acl++;
		if (acl == 0) {
			ach = ++ach & (SIGN|Q|P|HMSK);
			if ((ach & HMSK) == 0)
				acoflo = 1;
		}
	}
}

void
mpy()
{
	unsigned char sign;
	int subcycle;

	if ((srh & SIGN) == (mqh & SIGN))
		sign = 0;
	else
		sign = SIGN;
	srh &= HMSK;
	mqh &= HMSK;
	ach = 0;
	acl = 0;
	subcycle = 0;

	if ((srh & HMSK) == 0 && srl == 0) {
		mqh = 0;
		mql = 0;
		DCYCLE();
	} else {
		while (shcnt) {
			if (mql & 1)
				add();
			mql >>= 1;
			if (mqh & 1)
				mql |= 020000000000;
			mqh = (mqh & SIGN) | ((mqh >> 1) & HMSK);
			if (acl & 1)
				mqh |= 04;
			acl >>= 1;
			if (ach & 1)
				acl |= 020000000000;
			ach = (ach & SIGN) | ((ach >> 1) & (P|HMSK));
			shcnt--;
			if (++subcycle == 3) {
				DCYCLE();
				subcycle = 0;
			}
		}
	}
	ach |= sign;
	mqh |= sign;
}

void
div()
{
	unsigned char sign, mqsign;
	int subcycle;

	if ((srh & SIGN) == (ach & SIGN))
		mqsign = 0;
	else
		mqsign = SIGN;
	srh = (srh & HMSK) | SIGN;
	subcycle = 0;

	sign = ach & SIGN;
	ach = ach & (Q|P|HMSK);

	if ((srh & HMSK) < (ach & (Q|P|HMSK)) ||
	    ((srh & HMSK) == (ach & (Q|P|HMSK)) && srl <= acl)) {
		divchk = 1;
		DCYCLE();
	} else {
		mqh = (mqh & HMSK) | mqsign;
		while (shcnt) {
			ach = (ach & SIGN) | ((ach << 1) & (Q|P|HMSK));
			if (acl & 020000000000)
				ach++;
			acl <<= 1;
			if (mqh & 04)
				acl++;
			mqh = (mqh & SIGN) | ((mqh << 1) & HMSK);
			if (mql & 020000000000)
				mqh++;
			mql <<= 1;
			if (ach & P)
				acoflo = 1;
			if ((srh & HMSK) < (ach & (Q|P|HMSK)) ||
			    ((srh & HMSK) == (ach & (Q|P|HMSK)) &&
			     srl <= acl)) {
				add();
				mql |= 1;
			}
			shcnt--;
			if (++subcycle == 3) {
				DCYCLE();
				subcycle = 0;
			}
		}
	}
	ach |= sign;
}

/*
 * Floating Point
 *
 * Routines to extract and insert separate copy of characteristic
 */


void
xchar()
{

	srchar = ((srh &      HMSK)  << 5) | ((srl & 037000000000) >> 27);
	srh &= SIGN;
	srl &= 000777777777;
	acchar = ((ach & (Q|P|HMSK)) << 5) | ((acl & 037000000000) >> 27);
	ach &= SIGN;
	acl &= 000777777777;
	mqchar = ((mqh & (Q|P|HMSK)) << 5) | ((mql & 037000000000) >> 27);
	mqh &= SIGN;
	mql &= 000777777777;
}

void
ichar()
{

	srh = (srh & SIGN) | ((srchar & 0340) >> 5);
	srl = ((srchar & 0037L) << 27) | (srl & 000777777777);
	ach = (ach & SIGN) | ((acchar & 01740) >> 5);
	acl = ((acchar & 0037L) << 27) | (acl & 000777777777);
	mqh = (mqh & SIGN) | ((mqchar & 0340) >> 5);
	mql = ((mqchar & 0037L) << 27) | (mql & 000777777777);
}

void
FCYCLE()
{
	cycle_count++;
	if (single_cycle) {
		ichar();
		console();
		xchar();
	}
	if (cycle_count >= next_steal) {
		ichar();
		steal_cycle();
		xchar();
	}
}

/*
 * Floating Point Ops
 */

void
fadd(int nrm)
{

	mqh = 0;
	mql = 0;
	xchar();
	FCYCLE();
	if (srchar < acchar) {
		if (ach & P)
			ach |= SIGN;
		adderh = srh;
		adderl = srl;
		adderchar = srchar;
		srh = ach & (SIGN|HMSK);
		srl = acl;
		srchar = acchar;
		ach = adderh;
		acl = adderl;
		acchar = adderchar;
	}
	mqh = ach;
	FCYCLE();
	if (srchar - acchar > 077) {
		acl = 0;
		acchar = srchar;
		FCYCLE();
	} else while (srchar > acchar) {
		mql >>= 1;
		if (acl & 1)
			mql |= 000400000000;
		acl >>= 1;
		acchar++;
		if (acchar & 0400)
			spill |= 006;
		FCYCLE();
	}
	if (ach == srh) {
		acl += srl;
		if (acl & 001000000000) {
			mql >>= 1;
			if (acl & 1)
				mql |= 000400000000;
			acl >>= 1;
			acchar++;
			if (acchar & 0400)
				spill |= 006;
		}
	} else {
		if (srl > acl) {
			ach = mqh = srh;
			if (mql == 0) {
				acl = srl - acl;
			} else {
				acl = srl - acl - 1;
				mql = (0-mql) & 000777777777;
			}
		} else {
			acl -= srl;
		}
	}
	FCYCLE();
	if (nrm) {
		if (acl == 0 && mql == 0) {
			acchar = 0;
		} else while ((acl & 000400000000) == 0) {
			acl = (acl & 000377777777) << 1;
			if (mql & 000400000000)
				acl |= 1;
			mql = (mql & 000377777777) << 1;
			acchar--;
			if (acchar & 0400)
				spill |= 002;
		}
	}
	if (nrm && acl == 0 && mql == 0) {
		mqchar = 0;
	} else {
		mqchar = acchar - 27;
		if (mqchar & 0400)
			spill |= 001;
	}
	ichar();
}

void
frnd()
{

	if (mql & 000400000000) {
		if ((acl & 000777777777) == 000777777777) {
			acl += 000000000001;
			if (acl == 0) {
				ach = ((ach + 1) & (Q|P|HMSK)) | (ach & SIGN);
				if ((ach & (Q|P)) == P)
					spill |= 006;
			}
			acl |= 000400000000;
		} else {
			acl += 000000000001;
		}
	}
	DCYCLE();
}

void
fmpy(int nrm)
{

	ach = 0;
	acl = 0;
	if ( (srh & HMSK) == 0 && srl == 0 ) {
		mqh &= SIGN;
		mql = 0;
		return;
	}
	xchar();
	acchar = srchar + mqchar - 128;
	if (acchar & 0400) {
		if (acchar & 01000)
			spill |= 002;
		else
			spill |= 006;
	}
	FCYCLE();

	shcnt = 27;
	while (shcnt) {
		if (mql & 1)
			acl += srl;
		mql >>= 1;
		if (acl & 1)
			mql |= 000400000000;
		acl >>= 1;
		shcnt--;
		FCYCLE();
	}

	ach = (srh ^ mqh) & SIGN;
	if (nrm) {
		if (acl == 0) {
			acchar = 0;
		} else {
			if ((acl & 000400000000) == 0) {
				acl <<= 1;
				if (mql & 000400000000)
					acl += 1;
				mql = (mql << 1) & 000777777777;
				acchar--;
				if (acchar & 0400)
					spill |= 002;
			}
		}
		FCYCLE();
	}
	if (nrm && acl == 0) {
		mqchar = 0;
	} else {
		mqchar = acchar - 27;
		if (mqchar & 0400)
			spill |= 001;
	}
	mqh = ach;
	ichar();
}

void
fdiv()
{

	mql = 0;
	mqh = (ach ^ srh) & SIGN;
	xchar();
	if (acl >= (srl << 1) || srl == 0) {
		divchk = 1;
		ichar();
		return;
	}
	if (acl == 0) {
		ach = 0;
		acchar = 0;
		acl = 0;
		ichar();
		return;
	}
	if (acl >= srl) {
		if (acl & 1)
			mql |= 000400000000;
		acl >>= 1;
		acchar++;
	}
	mqchar = acchar - srchar + 128;
	if (mqchar & 0400) {
		if (mqchar & 01000)
			spill |= 001;
		else
			spill |= 005;
	}
	FCYCLE();

	shcnt = 27;
	while (shcnt) {
		acl <<= 1;
		if (mql & 000400000000)
			acl++;
		mql = (mql << 1) & 000777777777;
		if (srl <= acl) {
			mql++;
			acl -= srl;
		}
		shcnt--;
		FCYCLE();
	}

	acchar -= 27;
	if (acchar & 0400)
		spill |= 002;
	ichar();
	if (spill)
		spill |= 010;
}

/*
 * Double Precision 
 */

void
dfadd(int nrm)
{
}

void
dfrnd()
{
}

void
dfmpy(int nrm)
{
}

void
dfdiv()
{
}
