#include <u.h>
#include <libc.h>
#include <bio.h>
#include <mach.h>
#define Extern extern
#include "power.h"

ulong   setfpscr(void);
void    setfpcc(double);
void    farith(ulong);
void    farith2(ulong);
void    fariths(ulong);
void    fcmp(ulong);
void    mtfsb1(ulong);
void    mcrfs(ulong);
void    mtfsb0(ulong);
void    mtfsf(ulong);
void    mtfsfi(ulong);
void    mffs(ulong);
void    mtfsf(ulong);

Inst    op59[] = {
[18] {fariths, "fdivs", Ifloat},
[20] {fariths, "fsubs", Ifloat},
[21] {fariths, "fadds", Ifloat},
[22] {unimp, "fsqrts", Ifloat},
[24] {unimp, "fres", Ifloat},
[25] {fariths, "fmuls", Ifloat},
[28] {fariths, "fmsubs", Ifloat},
[29] {fariths, "fmadds", Ifloat},
[30] {fariths, "fnmsubs", Ifloat},
[31] {fariths, "fnmadds", Ifloat},
};

Inset   ops59 = {op59, nelem(op59)};

Inst    op63a[] = {
[12] {farith, "frsp", Ifloat},
[14] {farith, "fctiw", Ifloat},
[15] {farith, "fctiwz", Ifloat},
[18] {farith, "fdiv", Ifloat},
[20] {farith, "fsub", Ifloat},
[21] {farith, "fadd", Ifloat},
[22] {unimp, "frsqrt", Ifloat},
[23] {unimp, "fsel", Ifloat},
[25] {farith, "fmul", Ifloat},
[26] {unimp, "frsqrte", Ifloat},
[28] {farith, "fmsub", Ifloat},
[29] {farith, "fmadd", Ifloat},
[30] {farith, "fnmsub", Ifloat},
[31] {farith, "fnmadd", Ifloat},
};

Inset   ops63a= {op63a, nelem(op63a)};

Inst    op63b[] = {
[0] {fcmp, "fcmpu", Ifloat},
[32] {fcmp, "fcmpo", Ifloat},
[38] {mtfsb1, "mtfsb1", Ifloat},
[40] {farith2, "fneg", Ifloat},
[64] {mcrfs, "mcrfs", Ifloat},
[70] {mtfsb0, "mtfsb0", Ifloat},
[72] {farith2, "fmr", Ifloat},
[134] {mtfsfi, "mtfsfi", Ifloat},
[136] {farith2, "fnabs", Ifloat},
[264] {farith2, "fabs", Ifloat},
[583] {mffs, "mffs", Ifloat},
[711] {mtfsf, "mtfsf", Ifloat},
};

Inset   ops63b = {op63b, nelem(op63b)};

void
fpreginit(void)
{
       int i;

       /* Normally initialised by the kernel */
       reg.fd[27] = 4503601774854144.0;
       reg.fd[29] = 0.5;
       reg.fd[28] = 0.0;
       reg.fd[30] = 1.0;
       reg.fd[31] = 2.0;
       for(i = 0; i < 27; i++)
               reg.fd[i] = reg.fd[28];
}

static double
v2fp(uvlong v)
{
       FPdbleword f;

       f.hi = v>>32;
       f.lo = v;
       return f.x;
}

static uvlong
fp2v(double d)
{
       FPdbleword f;

       f.x = d;
       return ((uvlong)f.hi<<32) | f.lo;
}

void
lfs(ulong ir)
{
       ulong ea;
       int imm, ra, rd, upd;
       union {
               ulong   i;
               float   f;
       } u;

       getairr(ir);
       ea = imm;
       upd = (ir&(1L<<26))!=0;
       if(ra) {
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
       } else {
               if(upd)
                       undef(ir);
       }
       if(trace)
               itrace("%s\tf%d,%ld(r%d) ea=%lux", ci->name, rd, imm, ra, ea);

       u.i = getmem_w(ea);
       reg.fd[rd] = u.f;
}

void
lfsx(ulong ir)
{
       ulong ea;
       int rd, ra, rb, upd;
       union {
               ulong   i;
               float   f;
       } u;

       getarrr(ir);
       ea = reg.r[rb];
       upd = ((ir>>1)&0x3FF)==567;
       if(ra){
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
               if(trace)
                       itrace("%s\tf%d,(r%d+r%d) ea=%lux", ci->name, rd, ra, rb, ea);
       } else {
               if(upd)
                       undef(ir);
               if(trace)
                       itrace("%s\tf%d,(r%d) ea=%lux", ci->name, rd, rb, ea);
       }

       u.i = getmem_w(ea);
       reg.fd[rd] = u.f;
}

void
lfd(ulong ir)
{
       ulong ea;
       int imm, ra, rd, upd;

       getairr(ir);
       ea = imm;
       upd = (ir&(1L<<26))!=0;
       if(ra) {
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
       } else {
               if(upd)
                       undef(ir);
       }
       if(trace)
               itrace("%s\tf%d,%ld(r%d) ea=%lux", ci->name, rd, imm, ra, ea);

       reg.fd[rd] = v2fp(getmem_v(ea));
}

void
lfdx(ulong ir)
{
       ulong ea;
       int rd, ra, rb, upd;

       getarrr(ir);
       ea = reg.r[rb];
       upd = ((ir>>1)&0x3FF)==631;
       if(ra){
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
               if(trace)
                       itrace("%s\tf%d,(r%d+r%d) ea=%lux", ci->name, rd, ra, rb, ea);
       } else {
               if(upd)
                       undef(ir);
               if(trace)
                       itrace("%s\tf%d,(r%d) ea=%lux", ci->name, rd, rb, ea);
       }

       reg.fd[rd] = v2fp(getmem_v(ea));
}

void
stfs(ulong ir)
{
       ulong ea;
       int imm, ra, rd, upd;
       union {
               float f;
               ulong w;
       } u;

       getairr(ir);
       ea = imm;
       upd = (ir&(1L<<26))!=0;
       if(ra) {
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
       } else {
               if(upd)
                       undef(ir);
       }
       if(trace)
               itrace("%s\tf%d,%ld(r%d) %lux=%g",
                                       ci->name, rd, imm, ra, ea, reg.fd[rd]);
       u.f = reg.fd[rd];       /* BUG: actual PPC conversion is more subtle than this */
       putmem_w(ea, u.w);
}

void
stfsx(ulong ir)
{
       ulong ea;
       int rd, ra, rb, upd;
       union {
               float   f;
               ulong   w;
       } u;

       getarrr(ir);
       ea = reg.r[rb];
       upd = getxo(ir)==695;
       if(ra){
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
               if(trace)
                       itrace("%s\tf%d,(r%d+r%d) %lux=%g", ci->name, rd, ra, rb, ea, (float)reg.fd[rd]);
       } else {
               if(upd)
                       undef(ir);
               if(trace)
                       itrace("%s\tf%d,(r%d) %lux=%g", ci->name, rd, rb, ea, (float)reg.fd[rd]);
       }

       u.f = reg.fd[rd];       /* BUG: actual PPC conversion is more subtle than this */
       putmem_w(ea, u.w);
}

void
stfd(ulong ir)
{
       ulong ea;
       int imm, ra, rd, upd;

       getairr(ir);
       ea = imm;
       upd = (ir&(1L<<26))!=0;
       if(ra) {
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
       } else {
               if(upd)
                       undef(ir);
       }
       if(trace)
               itrace("%s\tf%d,%ld(r%d) %lux=%g",
                                       ci->name, rd, imm, ra, ea, reg.fd[rd]);

       putmem_v(ea, fp2v(reg.fd[rd]));
}

void
stfdx(ulong ir)
{
       ulong ea;
       int rd, ra, rb, upd;

       getarrr(ir);
       ea = reg.r[rb];
       upd = ((ir>>1)&0x3FF)==759;
       if(ra){
               ea += reg.r[ra];
               if(upd)
                       reg.r[ra] = ea;
               if(trace)
                       itrace("%s\tf%d,(r%d+r%d) %lux=%g", ci->name, rd, ra, rb, ea, reg.fd[rd]);
       } else {
               if(upd)
                       undef(ir);
               if(trace)
                       itrace("%s\tf%d,(r%d) %lux=%g", ci->name, rd, rb, ea, reg.fd[rd]);
       }

       putmem_v(ea, fp2v(reg.fd[rd]));
}

void
mcrfs(ulong ir)
{
       ulong rd, ra, rb;
       static ulong fpscr0[] ={
               FPS_FX|FPS_OX,
               FPS_UX|FPS_ZX|FPS_XX|FPS_VXSNAN,
               FPS_VXISI|FPS_VXIDI|FPS_VXZDZ|FPS_VXIMZ,
               FPS_VXVC,
               0,
               FPS_VXCVI,
       };

       getarrr(ir);
       if(rb || ra&3 || rd&3)
               undef(ir);
       ra >>= 2;
       rd >>= 2;
       reg.cr = (reg.cr & ~mkCR(rd, 0xF)) | mkCR(rd, getCR(ra, reg.fpscr));
       reg.fpscr &= ~fpscr0[ra];
       if(trace)
               itrace("mcrfs\tcrf%d,crf%d\n", rd, ra);
}

void
mffs(ulong ir)
{
       int rd, ra, rb;
       FPdbleword d;

       getarrr(ir);
       if(ra || rb)
               undef(ir);
       d.hi = 0xFFF80000UL;
       d.lo = reg.fpscr;
       reg.fd[rd] = d.x;
       /* it's anyone's guess how CR1 should be set when ir&1 */
       reg.cr &= ~mkCR(1, 0xE);        /* leave SO, reset others */
       if(trace)
               itrace("mffs%s\tfr%d\n", ir&1?".":"", rd);
}

void
mtfsb1(ulong ir)
{
       int rd, ra, rb;

       getarrr(ir);
       if(ra || rb)
               undef(ir);
       reg.fpscr |= (1L << (31-rd));
       /* BUG: should set summary bits */
       if(ir & 1)
               reg.cr &= ~mkCR(1, 0xE);        /* BUG: manual unclear: leave SO, reset others? */
       if(trace)
               itrace("mtfsb1%s\tfr%d\n", ir&1?".":"", rd);
}

void
mtfsb0(ulong ir)
{
       int rd, ra, rb;

       getarrr(ir);
       if(ra || rb)
               undef(ir);
       reg.fpscr &= ~(1L << (31-rd));
       if(ir & 1)
               reg.cr &= ~mkCR(1, 0xE);                /* BUG: manual unclear: leave SO, reset others? */
       if(trace)
               itrace("mtfsb0%s\tfr%d\n", ir&1?".":"", rd);
}

void
mtfsf(ulong ir)
{
       int fm, rb, i;
       FPdbleword d;
       ulong v;

       if(ir & ((1L << 25)|(1L << 16)))
               undef(ir);
       rb = (ir >> 11) & 0x1F;
       fm = (ir >> 17) & 0xFF;
       d.x = reg.fd[rb];
       v = d.lo;
       for(i=0; i<8; i++)
               if(fm & (1 << (7-i)))
                       reg.fpscr = (reg.fpscr & ~mkCR(i, 0xF)) | mkCR(i, getCR(i, v));
       /* BUG: should set FEX and VX `according to the usual rule' */
       if(ir & 1)
               reg.cr &= ~mkCR(1, 0xE);                /* BUG: manual unclear: leave SO, reset others? */
       if(trace)
               itrace("mtfsf%s\t#%.2x,fr%d", ir&1?".":"", fm, rb);
}

void
mtfsfi(ulong ir)
{
       int imm, rd;

       if(ir & ((0x7F << 16)|(1L << 11)))
               undef(ir);
       rd = (ir >> 23) & 0xF;
       imm = (ir >> 12) & 0xF;
       reg.fpscr = (reg.fpscr & ~mkCR(rd, 0xF)) | mkCR(rd, imm);
       /* BUG: should set FEX and VX `according to the usual rule' */
       if(ir & 1)
               reg.cr &= ~mkCR(1, 0xE);                /* BUG: manual unclear: leave SO, reset others? */
       if(trace)
               itrace("mtfsfi%s\tcrf%d,#%x", ir&1?".":"", rd, imm);
}

void
fcmp(ulong ir)
{
       int fc, rd, ra, rb;

       getarrr(ir);
       if(rd & 3)
               undef(ir);
       rd >>= 2;
       SET(fc);
       switch(getxo(ir)) {
       default:
               undef(ir);
       case 0:
               if(trace)
                       itrace("fcmpu\tcr%d,f%d,f%d", rd, ra, rb);
               if(isNaN(reg.fd[ra]) || isNaN(reg.fd[rb])) {
                       fc = CRFU;
                       break;
               }
               if(reg.fd[ra] == reg.fd[rb]) {
                       fc = CREQ;
                       break;
               }
               if(reg.fd[ra] < reg.fd[rb]) {
                       fc = CRLT;
                       break;
               }
               if(reg.fd[ra] > reg.fd[rb]) {
                       fc = CRGT;
                       break;
               }
               print("qi: fcmp error\n");
               break;
       case 32:
               if(trace)
                       itrace("fcmpo\tcr%d,f%d,f%d", rd, ra, rb);
               if(isNaN(reg.fd[ra]) || isNaN(reg.fd[rb])) {    /* BUG: depends whether quiet or signalling ... */
                       fc = CRFU;
                       Bprint(bioout, "invalid_fp_register\n");
                       longjmp(errjmp, 0);
               }
               if(reg.fd[ra] == reg.fd[rb]) {
                       fc = CREQ;
                       break;
               }
               if(reg.fd[ra] < reg.fd[rb]) {
                       fc = CRLT;
                       break;
               }
               if(reg.fd[ra] > reg.fd[rb]) {
                       fc = CRGT;
                       break;
               }
               print("qi: fcmp error\n");
               break;

       }
       fc >>= 28;
       reg.cr = (reg.cr & ~mkCR(rd,~0)) | mkCR(rd, fc);
       reg.fpscr = (reg.fpscr & ~0xF800) | (fc<<11);
       /* BUG: update FX, VXSNAN, VXVC */
}

/*
* the farith functions probably don't produce the right results
* in the presence of NaNs, Infs, etc., esp. wrt exception handling,
*/
void
fariths(ulong ir)
{
       int rd, ra, rb, rc, fmt;
       char *cc;
       ulong fpscr;

       fmt = 0;
       rc = (ir>>6)&0x1F;
       getarrr(ir);
       switch(getxo(ir)&0x1F) {        /* partial XO decode */
       default:
               undef(ir);
       case 18:
               if((float)reg.fd[rb] == 0.0) {
                       Bprint(bioout, "fp_exception ZX\n");
                       reg.fpscr |= FPS_ZX | FPS_FX;
                       longjmp(errjmp, 0);
               }
               reg.fd[rd] = (float)(reg.fd[ra] / reg.fd[rb]);
               break;
       case 20:
               reg.fd[rd] = (float)(reg.fd[ra] - reg.fd[rb]);
               break;
       case 21:
               reg.fd[rd] = (float)(reg.fd[ra] + reg.fd[rb]);
               break;
       case 25:
               reg.fd[rd] = (float)(reg.fd[ra] * reg.fd[rc]);
               rb = rc;
               break;
       case 28:
               reg.fd[rd] = (float)((reg.fd[ra] * reg.fd[rc]) - reg.fd[rb]);
               fmt = 2;
               break;
       case 29:
               reg.fd[rd] = (float)((reg.fd[ra] * reg.fd[rc]) + reg.fd[rb]);
               fmt = 2;
               break;
       case 30:
               reg.fd[rd] = (float)-((reg.fd[ra] * reg.fd[rc]) - reg.fd[rb]);
               fmt = 2;
               break;
       case 31:
               reg.fd[rd] = (float)-((reg.fd[ra] * reg.fd[rc]) + reg.fd[rb]);
               fmt = 2;
               break;
       }
       if(fmt==1 && ra)
               undef(ir);
       fpscr = setfpscr();
       setfpcc(reg.fd[rd]);
       cc = "";
       if(ir & 1) {
               cc = ".";
               reg.cr = (reg.cr & ~mkCR(1, ~0)) | mkCR(1, (fpscr>>28));
       }
       if(trace) {
               switch(fmt) {
               case 0:
                       itrace("%s%s\tfr%d,fr%d,fr%d", ci->name, cc, rd, ra, rb);
                       break;
               case 1:
                       itrace("%s%s\tfr%d,fr%d", ci->name, cc, rd, rb);
                       break;
               case 2:
                       itrace("%s%s\tfr%d,fr%d,fr%d,fr%d", ci->name, cc, rd, ra, rc, rb);
                       break;
               }
       }
}

void
farith(ulong ir)
{
       vlong vl;
       int rd, ra, rb, rc, fmt;
       char *cc;
       ulong fpscr;
       int nocc;
       double d;

       fmt = 0;
       nocc = 0;
       rc = (ir>>6)&0x1F;
       getarrr(ir);
       switch(getxo(ir)&0x1F) { /* partial XO decode */
       default:
               undef(ir);
       case 12:        /* frsp */
               reg.fd[rd] = (float)reg.fd[rb];
               fmt = 1;
               break;
       case 14:        /* fctiw */     /* BUG: ignores rounding mode */
       case 15:        /* fctiwz */
               d = reg.fd[rb];
               if(d >= 0x7fffffff)
                       vl = 0x7fffffff;
               else if(d < 0x80000000)
                       vl = 0x80000000;
               else
                       vl = d;
               reg.fd[rd] = v2fp(vl);
               fmt = 1;
               nocc = 1;
               break;
       case 18:
               if(reg.fd[rb] == 0.0) {
                       Bprint(bioout, "fp_exception ZX\n");
                       reg.fpscr |= FPS_ZX | FPS_FX;
                       longjmp(errjmp, 0);
               }
               reg.fd[rd] = reg.fd[ra] / reg.fd[rb];
               break;
       case 20:
               reg.fd[rd] = reg.fd[ra] - reg.fd[rb];
               break;
       case 21:
               reg.fd[rd] = reg.fd[ra] + reg.fd[rb];
               break;
       case 25:
               reg.fd[rd] = reg.fd[ra] * reg.fd[rc];
               rb = rc;
               break;
       case 28:
               reg.fd[rd] = (reg.fd[ra] * reg.fd[rc]) - reg.fd[rb];
               fmt = 2;
               break;
       case 29:
               reg.fd[rd] = (reg.fd[ra] * reg.fd[rc]) + reg.fd[rb];
               fmt = 2;
               break;
       case 30:
               reg.fd[rd] = -((reg.fd[ra] * reg.fd[rc]) - reg.fd[rb]);
               fmt = 2;
               break;
       case 31:
               reg.fd[rd] = -((reg.fd[ra] * reg.fd[rc]) + reg.fd[rb]);
               fmt = 2;
               break;
       }
       if(fmt==1 && ra)
               undef(ir);
       fpscr = setfpscr();
       if(nocc == 0)
               setfpcc(reg.fd[rd]);
       cc = "";
       if(ir & 1) {
               cc = ".";
               reg.cr = (reg.cr & ~mkCR(1, ~0)) | mkCR(1, (fpscr>>28));
       }
       if(trace) {
               switch(fmt) {
               case 0:
                       itrace("%s%s\tfr%d,fr%d,fr%d", ci->name, cc, rd, ra, rb);
                       break;
               case 1:
                       itrace("%s%s\tfr%d,fr%d", ci->name, cc, rd, rb);
                       break;
               case 2:
                       itrace("%s%s\tfr%d,fr%d,fr%d,fr%d", ci->name, cc, rd, ra, rc, rb);
                       break;
               }
       }
}

void
farith2(ulong ir)
{
       int rd, ra, rb;
       char *cc;
       ulong fpscr;

       getarrr(ir);
       switch(getxo(ir)) { /* full XO decode */
       default:
               undef(ir);
       case 40:
               reg.fd[rd] = -reg.fd[rb];
               break;
       case 72:
               reg.fd[rd] = reg.fd[rb];
               break;
       case 136:
               reg.fd[rd] = -fabs(reg.fd[rb]);
               break;
       case 264:
               reg.fd[rd] = fabs(reg.fd[rb]);
               break;
       }
       if(ra)
               undef(ir);
       fpscr = setfpscr();
       setfpcc(reg.fd[rd]);
       cc = "";
       if(ir & 1) {
               cc = ".";
               reg.cr = (reg.cr & ~mkCR(1, ~0)) | mkCR(1, (fpscr>>28));
       }
       if(trace)
               itrace("%s%s\tfr%d,fr%d", ci->name, cc, rd, rb);
}

ulong
setfpscr(void)
{
       ulong fps, fpscr;

       fps = getfsr();
       fpscr = reg.fpscr;
       if(fps & FPAOVFL)
               fpscr |= FPS_OX;
       if(fps & FPAINEX)
               fpscr |= FPS_XX;
       if(fps & FPAUNFL)
               fpscr |= FPS_UX;
       if(fps & FPAZDIV)
               fpscr |= FPS_ZX;
       if(fpscr != reg.fpscr) {
               fpscr |= FPS_FX;
               reg.fpscr = fpscr;
       }
       return fpscr;
}

void
setfpcc(double r)
{
       int c;

       c = 0;
       if(r == 0)
               c |= 2;
       else if(r < 0)
               c |= 4;
       else
               c |= 8;
       if(isNaN(r))
               c |= 1;
       reg.fpscr = (reg.fpscr & ~0xF800) | (0<<15) | (c<<11); /* unsure about class bit */
}

You are viewing proxied material from sdf.org. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.