kolibrios/contrib/other/uarm/pxa255_DSP.c

81 lines
1.9 KiB
C
Raw Normal View History

#include "math64.h"
#include "pxa255_DSP.h"
Boolean pxa255dspAccess(struct ArmCpu* cpu, void* userData, Boolean MRRC, UInt8 op, UInt8 RdLo, UInt8 RdHi, UInt8 acc){
Pxa255dsp* dsp = userData;
if(acc != 0 || op != 0) return false; //bad encoding
if(MRRC){ //MRA: read acc0
cpuSetReg(cpu, RdLo, u64_64_to_32(dsp->acc0));
cpuSetReg(cpu, RdHi, (UInt8)u64_get_hi(dsp->acc0));
}
else{ //MAR: write acc0
dsp->acc0 = u64_from_halves(cpuGetRegExternal(cpu, RdHi) & 0xFF, cpuGetRegExternal(cpu, RdLo));
}
return true;
}
Boolean pxa255dspOp(struct ArmCpu* cpu, void* userData, Boolean two/* MCR2/MRC2 ? */, Boolean MRC, UInt8 op1, UInt8 Rs, UInt8 opcode_3, UInt8 Rm, UInt8 acc){
Pxa255dsp* dsp = userData;
UInt64 addend = u64_zero();
UInt32 Vs, Vm;
if(op1 != 1 || two || MRC || acc != 0) return false; //bad encoding
Vs = cpuGetRegExternal(cpu, Rs);
Vm = cpuGetRegExternal(cpu, Rm);
switch(opcode_3 >> 2){
case 0: //MIA
addend = u64_smul3232(Vm, Vs);
break;
case 1: //invalid
return false;
case 2: //MIAPH
addend = u64_smul3232((Int32)((Int16)(Vm >> 0)), (Int32)((Int16)(Vs >> 0)));
addend = u64_add(addend, u64_smul3232((Int32)((Int16)(Vm >> 16)), (Int32)((Int16)(Vs >> 16))));
break;
case 3: //MIAxy
if(opcode_3 & 2) Vm >>= 16; //X set
if(opcode_3 & 1) Vs >>= 16; //Y set
addend = u64_smul3232((Int32)((Int16)Vm), (Int32)((Int16)Vs));
break;
}
dsp->acc0 = u64_and(u64_add(dsp->acc0, addend), u64_from_halves(0xFF, 0xFFFFFFFFUL));
return true;
}
Boolean pxa255dspInit(Pxa255dsp* dsp, ArmCpu* cpu){
ArmCoprocessor cp;
__mem_zero(dsp, sizeof(Pxa255dsp));
cp.regXfer = pxa255dspOp;
cp.dataProcessing = NULL;
cp.memAccess = NULL;
cp.twoRegF = pxa255dspAccess;
cp.userData = dsp;
cpuCoprocessorRegister(cpu, 0, &cp);
return true;
}