#include <thread.h>

#include <u.h>
#include <libc.h>
#include <thread.h>
#include <bio.h>
#include <mach.h>
#include "dat.h"
#include "fns.h"

void
resetfpa(void)
{
int i;

P->FPSR = 0x81000000;
for(i = 0; i < 8; i++)
P->F[i] = 0;
}

void
fpatransfer(u32int instr)
{
enum {
fP = 1<<24,
fU = 1<<23,
fT1 = 1<<22,
fW = 1<<21,
fL = 1<<20,
fT0 = 1<<15,
};

long double *Fd;
u32int *Rn, addr;
int off;
void *targ;
Segment *seg;

Rn = P->R + ((instr >> 16) & 15);
Fd = P->F + ((instr >> 12) & 7);
if(Rn == P->R + 15)
invalid(instr);
off = (instr & 255) * 4;
if(!(instr & fU))
off = -off;
addr = *Rn;
if(instr & fP)
addr += off;
targ = vaddr(addr, 8, &seg);
switch(instr & (fT0 | fT1 | fL)) {
case 0: *(float *) targ = *Fd; break;
case fL: *Fd = *(float *) targ; break;
case fT0: *(double *) targ = *Fd; break;
case fT0 | fL: *Fd = *(double *) targ; break;
default: invalid(instr);
}
segunlock(seg);
if(!(instr & fP))
addr += off;
if(instr & fW)
*Rn = addr;
}

static long double
fpasecop(u32int instr)
{
switch(instr & 15) {
case 8: return 0.0; break;
case 9: return 1.0; break;
case 10: return 2.0; break;
case 11: return 3.0; break;
case 12: return 4.0; break;
case 13: return 5.0; break;
case 14: return 0.5; break;
case 15: return 10.0; break;
}
return P->F[instr & 7];
}

void
fpaoperation(u32int instr)
{
long double *Fn, *Fd, op, op2, res;
int prec, opc;

Fn = P->F + ((instr >> 16) & 7);
Fd = P->F + ((instr >> 12) & 7);
op2 = fpasecop(instr);
op = *Fn;
prec = ((instr >> 7) & 1) | ((instr >> 18) & 2);
opc = ((instr >> 20) & 15) | ((instr >> 11) & 16);
switch(opc) {
case 0: res = op + op2; break;
case 1: res = op * op2; break;
case 2: res = op - op2; break;
case 3: res = op2 - op; break;
case 4: res = op / op2; break;
case 5: res = op2 / op; break;
case 16: res = op2; break;
case 17: res = - op2; break;
case 18: res = fabs(op2); break;
case 19: res = (vlong) op2; break;
case 20: res = sqrt(op2); break;
default: sysfatal("unimplemented FPA operation %#x @ %8ux", opc, P->R[15] - 4);
return;
}
switch(prec) {
case 0: *Fd = (float) res; break;
case 1: *Fd = (double) res; break;
case 2: *Fd = res; break;
default: invalid(instr);
}
}

void
fparegtransfer(u32int instr)
{
u32int *Rd;
long tmp;
long double *Fn, op, op2;

Rd = P->R + ((instr >> 12) & 15);
Fn = P->F + ((instr >> 16) & 7);
op = fpasecop(instr);
if(Rd == P->R + 15) {
op2 = *Fn;
switch((instr >> 21) & 7) {
case 4: break;
case 5: op = - op; break;
default: invalid(instr);
}
if(op2 < op)
P->CPSR = (P->CPSR & ~FLAGS) | flN;
else if(op2 >= op) {
P->CPSR = (P->CPSR & ~FLAGS) | flC;
if(op2 == op)
P->CPSR |= flZ;
} else
P->CPSR = (P->CPSR & ~FLAGS) | flV;
return;
}
if(instr & (1<<3))
invalid(instr);
switch((instr >> 20) & 15) {
case 0: *Fn = *(long *) Rd; break;
case 1: tmp = op; *Rd = tmp; break;
case 2: P->FPSR = *Rd; break;
case 3: *Rd = P->FPSR; break;
default: invalid(instr);
}
}