shithub: drawcpu

ref: 23109a6631eb21b2d849807b470c239d71d09159
dir: /kern/vfp.c/

#include <u.h>
#include <lib.h>
#include "dat.h"
#include "fns.h"
#include "proc.h"

void
resetvfp(void)
{
	int i;

	up->FPSR = 0x00000000;
	for(i = 0; i < Nfpregs; i++)
		up->F[i] = 0;
}

void
vfpregtransfer(u32int instr)
{
	u32int *Rt;
	double *Fn;
	Rt = up->R + ((instr>>12)&0xF);
	Fn = up->F + ((instr>>16)&0xF);
	switch((instr>>20)&0xF){
	case 0:
		*((int*)Fn) = *Rt; break;
	case 1:
		*Rt = *((int*)Fn); break;
	case 14:
		up->FPSR = *Rt; break;
	case 15:
		if(Rt == (up->R + 15))
			up->CPSR = up->FPSR;
		else
			*Rt = up->FPSR;
		break;
	default:
		sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, up->R[15] - 4);
	}
}

void
vfprmtransfer(u32int instr)
{
	int n, d, off, sz;
	void* ea;
	Segment *seg;
	n = (instr>>16) & 0xF;
	d = (instr>>12) & 0xF;
	off = (instr & 0xFF) << 2;
	sz = instr & (1<<8);
	if((instr & (1<<23)) == 0)
		off = -off;
	ea = vaddr(evenaddr(up->R[n] + off, 3), sz ? 8 : 4, &seg);
	switch((instr>>20)&0x3){
	case 0:
		if(sz)
			*(double*)ea = up->F[d];
		else
			*(float*)ea = up->F[d];
		break;
	case 1:
		if(sz)
			up->F[d] = *(double*)ea;
		else
			up->F[d] = *(float*)ea;
		break;
	case 2:
        if(sz)
            *(double*)ea = up->F[d];
        else
            *(float*)ea = up->F[d];
		up->R[n] += off;
        break;
	default:
		sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, up->R[15] - 4);
	}
	segunlock(seg);
}

void
vfparithop(int opc, u32int instr)
{
	int o;
	double *Fd, *Fn, *Fm;

	Fd = up->F + ((instr>>12)&0xF);
	Fn = up->F + ((instr>>16)&0xF);
	Fm = up->F + (instr&0xF);
	o = ((opc&0x3)<<1) | (opc&0x8) | ((instr>>6)&0x1);

	switch(o){
	case 4:
		*Fd = *Fn * *Fm; break;
	case 6:
		*Fd = *Fn + *Fm; break;
	case 7:
		*Fd = *Fn - *Fm; break;
	case 8:
		*Fd = *Fn / *Fm; break;
	default:
		sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, up->R[15] - 4);
	}
}

void
vfpotherop(u32int instr)
{
	int o2, o3;
	double *Fd, *Fm, F0;

	Fd = up->F + ((instr>>12)&0xF);
	Fm = up->F + (instr&0xF);
	F0 = 0.0;
	o2 = (instr>>16) & 0xF;
	o3 = (instr>>6) & 0x3;

	if((o3&1) == 0)
		sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, up->R[15] - 4);
	switch(o2){
	case 0x5:
		Fm = &F0;
	case 0x4:
		if(*Fd < *Fm)
			up->FPSR = (up->FPSR & ~FLAGS) | flN;
		else if(*Fd >= *Fm) {
			up->FPSR = (up->FPSR & ~FLAGS) | flC;
			if(*Fd == *Fm)
				up->FPSR |= flZ;
		} else
			up->FPSR = (up->FPSR & ~FLAGS) | flV | flC;
		break;
	case 0x8:
		*Fd = *((int*)Fm); break;
	case 0xD:
		*((int*)Fd) = (int)*Fm; break;
	default:
		switch((o2<<1)|(o3>>1)){
		case 0:
		case 15:
			*Fd = *Fm; break;
		case 1:
			*Fd = fabs(*Fm); break;
		case 2:
			*Fd = -*Fm; break;
		case 3:
			*Fd = sqrt(*Fm); break;
		default:
			sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, up->R[15] - 4);
		}
	}
}

void
vfpoperation(u32int instr)
{
	int o1;
	o1 = (instr>>20) & 0xF;
	if(o1 == 0xB)
		vfpotherop(instr);
	else
		vfparithop(o1, instr);
}