/***********************************************************************
*
* arith.c - IBM 7090 emulator arithmetic routines.
*
* Changes:
*   ??/??/??   PRP   Original.
*   12/20/04   DGP   Added double precision.
*   02/07/05   DGP   Added check for negative MQEXP calc and fixed FMP
*                  MQ == 0.
*   
***********************************************************************/

#define EXTERN extern

#include <stdio.h>

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

uint16 srexp;
uint16 sr2exp;
uint16 acexp;
uint16 mqexp;
uint16 addrexp;

uint8 srfrach;   uint32 srfracl;
uint8 sr2frach;  uint32 sr2fracl;
uint8 acfrach;   uint32 acfracl;
uint8 mqfrach;   uint32 mqfracl;
uint8 addrfrach; uint32 addrfracl;

/*
 * 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()
{
   uint8 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()
{
   uint8 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()
{

   srexp = ((srh & HMSK)  << 5) | ((srl & 037000000000) >> 27);
   srfrach = srh & SIGN;
   srfracl = srl & 000777777777;

   acexp = ((ach & (Q|P|HMSK)) << 5) | ((acl & 037000000000) >> 27);
   acfrach = ach & SIGN;
   acfracl = acl & 000777777777;

   mqexp = ((mqh & (Q|P|HMSK)) << 5) | ((mql & 037000000000) >> 27);
   mqfrach = mqh & SIGN;
   mqfracl = mql & 000777777777;
}

void
dxchar()
{

   xchar();
   sr2exp = ((sr2h & HMSK)  << 5) | ((sr2l & 037000000000) >> 27);
   sr2frach = sr2h & SIGN;
   sr2fracl = sr2l & 000777777777;
}

void
ichar()
{

   srh = (srfrach & SIGN) | ((srexp & 0340) >> 5);
   srl = ((srexp & 0037L) << 27) | (srfracl & 000777777777);

   ach = (acfrach & SIGN) | ((acexp & 01740) >> 5);
   acl = ((acexp & 0037L) << 27) | (acfracl & 000777777777);

   mqh = (mqfrach & SIGN) | ((mqexp & 0340) >> 5);
   mql = ((mqexp & 0037L) << 27) | (mqfracl & 000777777777);
}

void
dichar()
{

   ichar();
   sr2h = (sr2frach & SIGN) | ((sr2exp & 0340) >> 5);
   sr2l = ((sr2exp & 0037L) << 27) | (sr2fracl & 000777777777);
}

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

void
DFCYCLE()
{
   cycle_count++;
   if (single_cycle)
   {
      dichar();
      console();
      dxchar();
   }
   if (cycle_count >= next_steal)
   {
      dichar();
      steal_cycle();
      dxchar();
   }
}

/*
 * Floating Point functions.
 */

void
dofadd(int nrm)
{
   mqfrach = 0;
   mqfracl = 0;
   if (srexp < acexp)
   {
      if (acfrach & P)
         acfrach |= SIGN;
      addrfrach = srfrach;
      addrfracl = srfracl;
      addrexp = srexp;
      srfrach = acfrach & (SIGN|HMSK);
      srfracl = acfracl;
      srexp = acexp;
      acfrach = addrfrach;
      acfracl = addrfracl;
      acexp = addrexp;
   }
   mqfrach = acfrach;
   if (srexp - acexp > 077)
   {
      acfracl = 0;
      acexp = srexp;
   }
   else while (srexp > acexp)
   {
      mqfracl >>= 1;
      if (acfracl & 1)
         mqfracl |= 000400000000;
      acfracl >>= 1;
      acexp++;
      if (acexp & 0400)
         spill |= 006;
   }
   if (acfrach == srfrach)
   {
      acfracl += srfracl;
      if (acfracl & 001000000000)
      {
         mqfracl >>= 1;
         if (acfracl & 1)
            mqfracl |= 000400000000;
         acfracl >>= 1;
         acexp++;
         if (acexp & 0400)
            spill |= 006;
      }
   }
   else
   {
      if (srfracl > acfracl)
      {
         acfrach = mqfrach = srfrach;
         if (mqfracl == 0)
	 {
            acfracl = srfracl - acfracl;
         }
	 else
	 {
            acfracl = srfracl - acfracl - 1;
            mqfracl = (0-mqfracl) & 000777777777;
         }
      }
      else
      {
         acfracl -= srfracl;
      }
   }
   if (nrm)
   {
      if (acfracl == 0 && mqfracl == 0)
      {
         acexp = 0;
      }
      else while ((acfracl & 000400000000) == 0)
      {
         acfracl = (acfracl & 000377777777) << 1;
         if (mqfracl & 000400000000)
            acfracl |= 1;
         mqfracl = (mqfracl & 000377777777) << 1;
         acexp--;
         if (acexp & 0400)
	 {
	    fprintf (stderr, "dofadd: AC underflow at %05o: acexp = %o\n", ic, acexp);
            spill |= 002;
	 }
      }
   }
   if (nrm && acfracl == 0 && mqfracl == 0)
   {
      mqexp = 0;
   }
   else
   {
      if (((int16)acexp - 27) < 0)
	 mqexp = 0;
      else
	 mqexp = acexp - 27;
      if (mqexp & 0400)
         spill |= 001;
   }
}

void
dofrnd()
{

   if (mqfracl & 000400000000)
   {
      if ((acfracl & 000777777777) == 000777777777)
      {
         acfracl += 000000000001;
         if (acfracl == 0)
	 {
            acfrach = ((acfrach + 1) & (Q|P|HMSK)) | (acfrach & SIGN);
            if ((acfrach & (Q|P)) == P)
               spill |= 006;
         }
         acfracl |= 000400000000;
      }
      else
      {
         acfracl += 000000000001;
      }
   }
   DCYCLE();
}

void
dofmpy(int nrm)
{

   acfrach = 0;
   acfracl = 0;
   /*
   ** If MQ == 0, return
   */
   if ( (mqfrach & HMSK) == 0 && mqfracl == 0 )
   {
      return;
   }
   /*
   ** If SR == 0, clear MQ & return
   */
   if ( (srfrach & HMSK) == 0 && srfracl == 0 )
   {
      mqfrach &= SIGN;
      mqfracl = 0;
      return;
   }
   acexp = srexp + mqexp - 128;
   if (acexp & 0400)
   {
      if (acexp & 01000)
         spill |= 002;
      else
         spill |= 006;
   }

   shcnt = 27;
   while (shcnt)
   {
      if (mqfracl & 1)
         acfracl += srfracl;
      mqfracl >>= 1;
      if (acfracl & 1)
         mqfracl |= 000400000000;
      acfracl >>= 1;
      shcnt--;
   }

   acfrach = (srfrach ^ mqfrach) & SIGN;
   if (nrm)
   {
      if (acfracl == 0)
      {
         acexp = 0;
      }
      else
      {
         if ((acfracl & 000400000000) == 0)
	 {
            acfracl <<= 1;
            if (mqfracl & 000400000000)
               acfracl += 1;
            mqfracl = (mqfracl << 1) & 000777777777;
            acexp--;
            if (acexp & 0400)
	    {
	       fprintf (stderr, "dofmpy-2: AC underflow at %05o: acexp = %o\n", ic, acexp);
               spill |= 002;
	    }
         }
      }
   }
   if (nrm && acfracl == 0)
   {
      mqexp = 0;
   }
   else
   {
      if (((int16)acexp - 27) < 0)
	 mqexp = 0;
      else
	 mqexp = acexp - 27;
      if (mqexp & 0400)
         spill |= 001;
   }
   mqfrach = acfrach;
}

void
dofdiv()
{

   mqfracl = 0;
   mqfrach = (acfrach ^ srfrach) & SIGN;
   if (acfracl >= (srfracl << 1) || srfracl == 0)
   {
      divchk = 1;
      return;
   }
   if (acfracl == 0)
   {
      acfrach = 0;
      acexp = 0;
      acfracl = 0;
      return;
   }
   if (acfracl >= srfracl)
   {
      if (acfracl & 1)
         mqfracl |= 000400000000;
      acfracl >>= 1;
      acexp++;
   }
   mqexp = acexp - srexp + 128;
   if (mqexp & 0400)
   {
      if (mqexp & 01000)
         spill |= 001;
      else
         spill |= 005;
   }

   shcnt = 27;
   while (shcnt)
   {
      acfracl <<= 1;
      if (mqfracl & 000400000000)
         acfracl++;
      mqfracl = (mqfracl << 1) & 000777777777;
      if (srfracl <= acfracl)
      {
         mqfracl++;
         acfracl -= srfracl;
      }
      shcnt--;
   }

   acexp -= 27;
   if (acexp & 0400)
   {
      fprintf (stderr, "dofdiv: AC underflow at %05o: acexp = %o\n", ic, acexp);
      spill |= 002;
   }
   if (spill)
      spill |= 010;
}

/*
 * Single Precision 
 */

void
fadd(int nrm)
{
   xchar();
   dofadd(nrm);
   ichar();
   FCYCLE();
}

void
frnd(int nrm)
{
   xchar();
   dofrnd(nrm);
   ichar();
   FCYCLE();
}

void
fmpy(int nrm)
{
   xchar();
   dofmpy(nrm);
   ichar();
   FCYCLE();
}

void
fdiv()
{
   xchar();
   dofdiv();
   ichar();;
   FCYCLE();
}

/*
 * Double Precision based on the 7090 macros.
 */

void
dfadd(int nrm)
{
   uint16 lsrexp; uint8 lsrh; uint32 lsrl;
   uint16 le1exp; uint8 le1h; uint32 le1l;
   uint16 le2exp; uint8 le2h; uint32 le2l;

   dxchar();
   lsrexp = srexp; lsrh = srfrach; lsrl = srfracl;

   /* STQ   e.1 */
   le1exp = mqexp; le1h = mqfrach; le1l = mqfracl;

   /* FAD   y,t */
   dofadd(nrm);

   /* STO   e.2 */
   le2exp = acexp; le2h = acfrach; le2l = acfracl;

   /* XCA       */
   srexp = acexp; srh = acfrach; srl = acfracl;
   acexp = mqexp; acfrach = mqfrach; acfracl = mqfracl;
   mqexp = srexp; mqfrach = srh; mqfracl = srl;

   /* FAD   e.1 */
   srexp = le1exp; srfrach = le1h; srfracl = le1l;
   dofadd(1);

   /* FAD   y+1,t */
   srexp = sr2exp; srfrach = sr2frach; srfracl = sr2fracl;
   dofadd(nrm);

   /* FAD   e.2 */
   srexp = le2exp; srfrach = le2h; srfracl = le2l;
   dofadd(1);

   srexp = lsrexp; srfrach = lsrh; srfracl = lsrl;
   dichar();
   DFCYCLE();
}

void
dfmpy(int nrm)
{
   uint16 lsrexp; uint8 lsrh; uint32 lsrl;
   uint16 le1exp; uint8 le1h; uint32 le1l;
   uint16 le2exp; uint8 le2h; uint32 le2l;
   uint16 le3exp; uint8 le3h; uint32 le3l;
   uint16 le4exp; uint8 le4h; uint32 le4l;

   dxchar();
   lsrexp = srexp; lsrh = srfrach; lsrl = srfracl;

   /* STO   e.1 */
   le1exp = acexp; le1h = acfrach; le1l = acfracl;

   /* FMP   y,t */
   dofmpy(1);

   /* STO   e.2 */
   le2exp = acexp; le2h = acfrach; le2l = acfracl;

   /* LDQ   y,t */
   mqexp = lsrexp; mqfrach = lsrh; mqfracl = lsrl;

   /* FMP   e.1 */
   srexp = le1exp; srfrach = le1h; srfracl = le1l;
   dofmpy(1);

   /* STQ   e.3 */
   le3exp = mqexp; le3h = mqfrach; le3l = mqfracl;

   /* STO   e.4 */
   le4exp = acexp; le4h = acfrach; le4l = acfracl;

   /* LDQ   y+1,t */
   mqexp = sr2exp; mqfrach = sr2h; mqfracl = sr2l;

   /* FMP   e.1 */
   srexp = le1exp; srfrach = le1h; srfracl = le1l;
   dofmpy(1);

   /* FAD   e.2 */
   srexp = le2exp; srfrach = le2h; srfracl = le2l;
   dofadd(nrm);

   /* FAD   e.3 */
   srexp = le3exp; srfrach = le3h; srfracl = le3l;
   dofadd(1);

   /* FAD   e.4 */
   srexp = le4exp; srfrach = le4h; srfracl = le4l;
   dofadd(1);

   srexp = lsrexp; srfrach = lsrh; srfracl = lsrl;
   dichar();
   DFCYCLE();
}

void
dfdiv()
{
   uint16 lsrexp; uint8 lsrh; uint32 lsrl;
   uint16 le1exp; uint8 le1h; uint32 le1l;
   uint16 le2exp; uint8 le2h; uint32 le2l;
   uint16 le3exp; uint8 le3h; uint32 le3l;
   uint16 le4exp; uint8 le4h; uint32 le4l;

   dxchar();
   lsrexp = srexp; lsrh = srfrach; lsrl = srfracl;

   /* STQ   e.1 */
   le1exp = mqexp; le1h = mqfrach; le1l = mqfracl;

   /* FDP   y,t */
   dofdiv();

   /* STO   e.2 */
   le2exp = acexp; le2h = acfrach; le2l = acfracl;

   /* STQ   e.3 */
   le3exp = mqexp; le3h = mqfrach; le3l = mqfracl;

   /* FMP   y+1,t */
   srexp = sr2exp; srfrach = sr2h; srfracl = sr2l;
   dofmpy(1);

   /* CHS       */
   acfrach = (~acfrach & SIGN) | (acfrach & (Q|P|HMSK));

   /* FAD   e.2 */
   srexp = le2exp; srfrach = le2h; srfracl = le2l;
   dofadd(1);

   /* FAD   e.1 */
   srexp = le1exp; srfrach = le1h; srfracl = le1l;
   dofadd(1);

   /* FDP   y,t */
   srexp = lsrexp; srfrach = lsrh; srfracl = lsrl;
   dofdiv();

   /* XCA       */
   srexp = acexp; srh = acfrach; srl = acfracl;
   acexp = mqexp; acfrach = mqfrach; acfracl = mqfracl;
   mqexp = srexp; mqfrach = srh; mqfracl = srl;

   /* FAD   e.3 */
   srexp = le3exp; srfrach = le3h; srfracl = le3l;
   dofadd(1);

   srexp = lsrexp; srfrach = lsrh; srfracl = lsrl;
   dichar();
   DFCYCLE();
}
