sys/src/cmd/5e/vfp.c

   1 #include <u.h>
   2 #include <libc.h>
   3 #include <thread.h>
   4 #include <bio.h>
   5 #include <mach.h>
   6 #include "dat.h"
   7 #include "fns.h"
   8
   9 void
  10 resetvfp(void)
  11 {
  12         int i;
  13
  14         P->FPSR = 0x00000000;
  15         for(i = 0; i < Nfpregs; i++)
  16                 P->F[i] = 0;
  17 }
  18
  19 void
  20 vfpregtransfer(u32int instr)
  21 {
  22         u32int *Rt;
  23         long double *Fn;
  24         Rt = P->R + ((instr>>12)&0xF);
  25         Fn = P->F + ((instr>>16)&0xF);
  26
  27         switch((instr>>20)&0xF){
  28         case 0:
  29                 *((int*)Fn) = *Rt; break;
  30         case 1:
  31                 *Rt = *((int*)Fn); break;
  32         case 14:
  33                 P->FPSR = *Rt; break;
  34         case 15:
  35                 if(Rt == (P->R + 15))
  36                         P->CPSR = P->FPSR;
  37                 else
  38                         *Rt = P->FPSR;
  39                 break;
  40         default:
  41                 sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, P->R[15] - 4);
  42         }
  43 }
  44
  45 void
  46 vfprmtransfer(u32int instr)
  47 {
  48         int n, d, off, sz;
  49         void* ea;
  50         Segment *seg;
  51
  52         n = (instr>>16) & 0xF;
  53         d = (instr>>12) & 0xF;
  54         off = (instr & 0xFF) << 2;
  55         sz = instr & (1<<8);
  56         if((instr & (1<<23)) == 0)
  57                 off = -off;
  58         ea = vaddr(evenaddr(P->R[n] + off, 3), sz ? 8 : 4, &seg);
  59         switch((instr>>20)&0x3){
  60         case 0:
  61                 if(sz)
  62                         *(double*)ea = P->F[d];
  63                 else
  64                         *(float*)ea = P->F[d];
  65                 break;
  66         case 1:
  67                 if(sz)
  68                         P->F[d] = *(double*)ea;
  69                 else
  70                         P->F[d] = *(float*)ea;
  71                 break;
  72         default:
  73                 sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, P->R[15] - 4);
  74         }
  75         segunlock(seg);
  76 }
  77
  78 void
  79 vfparithop(int opc, u32int instr)
  80 {
  81         int o;
  82         long double *Fd, *Fn, *Fm;
  83         Fd = P->F + ((instr>>12)&0xF);
  84         Fn = P->F + ((instr>>16)&0xF);
  85         Fm = P->F + (instr&0xF);
  86         o = ((opc&0x3)<<1) | (opc&0x8) | ((instr>>6)&0x1);
  87
  88         switch(o){
  89         case 4:
  90                 *Fd = *Fn * *Fm; break;
  91         case 6:
  92                 *Fd = *Fn + *Fm; break;
  93         case 7:
  94                 *Fd = *Fn - *Fm; break;
  95         case 8:
  96                 *Fd = *Fn / *Fm; break;
  97         default:
  98                 sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, P->R[15] - 4);
  99         }
 100 }
 101
 102 void
 103 vfpotherop(u32int instr)
 104 {
 105         int o2, o3;
 106         long double *Fd, *Fm, F0;
 107         Fd = P->F + ((instr>>12)&0xF);
 108         Fm = P->F + (instr&0xF);
 109         F0 = 0.0;
 110         o2 = (instr>>16) & 0xF;
 111         o3 = (instr>>6) & 0x3;
 112
 113         if((o3&1) == 0)
 114                 sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, P->R[15] - 4);
 115         switch(o2){
 116         case 0x5:
 117                 Fm = &F0;
 118         case 0x4:
 119                 if(*Fd < *Fm)
 120                         P->FPSR = (P->FPSR & ~FLAGS) | flN;
 121                 else if(*Fd >= *Fm) {
 122                         P->FPSR = (P->FPSR & ~FLAGS) | flC;
 123                         if(*Fd == *Fm)
 124                                 P->FPSR |= flZ;
 125                 } else
 126                         P->FPSR = (P->FPSR & ~FLAGS) | flV | flC;
 127                 break;
 128         case 0x8:
 129                 *Fd = *((int*)Fm); break;
 130         case 0xD:
 131                 *((int*)Fd) = (int)*Fm; break;
 132         default:
 133                 switch((o2<<1)|(o3>>1)){
 134                 case 0:
 135                 case 15:
 136                         *Fd = *Fm; break;
 137                 case 1:
 138                         *Fd = fabs(*Fm); break;
 139                 case 2:
 140                         *Fd = -*Fm; break;
 141                 case 3:
 142                         *Fd = sqrt(*Fm); break;
 143                 default:
 144                         sysfatal("unimplemented VFP instruction %8ux @ %8ux", instr, P->R[15] - 4);
 145                 }
 146         }
 147 }
 148
 149 void
 150 vfpoperation(u32int instr)
 151 {
 152         int o1;
 153         o1 = (instr>>20) & 0xF;
 154         if(o1 == 0xB)
 155                 vfpotherop(instr);
 156         else
 157                 vfparithop(o1, instr);
 158 }
 159