Subversion Repositories Kolibri OS

Rev

Blame | Last modification | View Log | RSS feed

  1. #include "math64.h"
  2. #include "pxa255_DSP.h"
  3.  
  4.  
  5.  
  6.  
  7. Boolean pxa255dspAccess(struct ArmCpu* cpu, void* userData, Boolean MRRC, UInt8 op, UInt8 RdLo, UInt8 RdHi, UInt8 acc){
  8.        
  9.         Pxa255dsp* dsp = userData;
  10.        
  11.         if(acc != 0 || op != 0) return false;                           //bad encoding
  12.        
  13.         if(MRRC){       //MRA: read acc0
  14.                
  15.                 cpuSetReg(cpu, RdLo, u64_64_to_32(dsp->acc0));
  16.                 cpuSetReg(cpu, RdHi, (UInt8)u64_get_hi(dsp->acc0));
  17.         }
  18.         else{           //MAR: write acc0
  19.                
  20.                 dsp->acc0 = u64_from_halves(cpuGetRegExternal(cpu, RdHi) & 0xFF, cpuGetRegExternal(cpu, RdLo));
  21.         }
  22.        
  23.         return true;   
  24. }
  25.  
  26. Boolean pxa255dspOp(struct ArmCpu* cpu, void* userData, Boolean two/* MCR2/MRC2 ? */, Boolean MRC, UInt8 op1, UInt8 Rs, UInt8 opcode_3, UInt8 Rm, UInt8 acc){
  27.        
  28.         Pxa255dsp* dsp = userData;
  29.         UInt64 addend = u64_zero();
  30.         UInt32 Vs, Vm;
  31.        
  32.         if(op1 != 1 || two || MRC || acc != 0) return false;            //bad encoding
  33.        
  34.         Vs = cpuGetRegExternal(cpu, Rs);
  35.         Vm = cpuGetRegExternal(cpu, Rm);
  36.        
  37.         switch(opcode_3 >> 2){
  38.                
  39.                 case 0: //MIA
  40.                         addend = u64_smul3232(Vm, Vs);
  41.                         break;
  42.                
  43.                 case 1: //invalid
  44.                         return false;
  45.                
  46.                 case 2: //MIAPH
  47.                         addend = u64_smul3232((Int32)((Int16)(Vm >>  0)), (Int32)((Int16)(Vs >>  0)));
  48.                         addend = u64_add(addend, u64_smul3232((Int32)((Int16)(Vm >> 16)), (Int32)((Int16)(Vs >> 16))));
  49.                         break;
  50.                
  51.                 case 3: //MIAxy
  52.                         if(opcode_3 & 2) Vm >>= 16;     //X set
  53.                         if(opcode_3 & 1) Vs >>= 16;     //Y set
  54.                         addend = u64_smul3232((Int32)((Int16)Vm), (Int32)((Int16)Vs));
  55.                         break;
  56.         }
  57.        
  58.         dsp->acc0 = u64_and(u64_add(dsp->acc0, addend), u64_from_halves(0xFF, 0xFFFFFFFFUL));
  59.        
  60.         return true;
  61. }
  62.  
  63.  
  64.  
  65. Boolean pxa255dspInit(Pxa255dsp* dsp, ArmCpu* cpu){
  66.        
  67.         ArmCoprocessor cp;
  68.        
  69.        
  70.         __mem_zero(dsp, sizeof(Pxa255dsp));
  71.        
  72.         cp.regXfer = pxa255dspOp;
  73.         cp.dataProcessing = NULL;
  74.         cp.memAccess = NULL;
  75.         cp.twoRegF = pxa255dspAccess;
  76.         cp.userData = dsp;
  77.        
  78.         cpuCoprocessorRegister(cpu, 0, &cp);
  79.  
  80.         return true;
  81. }