sys/src/9/bcm/vfp3.c

   1 /*
   2  * VFPv2 or VFPv3 floating point unit
   3  */
   4 #include "u.h"
   5 #include "../port/lib.h"
   6 #include "mem.h"
   7 #include "dat.h"
   8 #include "fns.h"
   9 #include "ureg.h"
  10 #include "arm.h"
  11
  12 /* subarchitecture code in m->havefp */
  13 enum {
  14         VFPv2   = 2,
  15         VFPv3   = 3,
  16 };
  17
  18 /* fp control regs.  most are read-only */
  19 enum {
  20         Fpsid = 0,
  21         Fpscr = 1,                      /* rw */
  22         Mvfr1 = 6,
  23         Mvfr0 = 7,
  24         Fpexc = 8,                      /* rw */
  25         Fpinst= 9,                      /* optional, for exceptions */
  26         Fpinst2=10,
  27 };
  28 enum {
  29         /* Fpexc bits */
  30         Fpex =          1u << 31,
  31         Fpenabled =     1 << 30,
  32         Fpdex =         1 << 29,        /* defined synch exception */
  33 //      Fp2v =          1 << 28,        /* Fpinst2 reg is valid */
  34 //      Fpvv =          1 << 27,        /* if Fpdex, vecitr is valid */
  35 //      Fptfv =         1 << 26,        /* trapped fault is valid */
  36 //      Fpvecitr =      MASK(3) << 8,
  37         /* FSR bits appear here */
  38         Fpmbc =         Fpdex,          /* bits exception handler must clear */
  39
  40         /* Fpscr bits; see u.h for more */
  41         Stride =        MASK(2) << 20,
  42         Len =           MASK(3) << 16,
  43         Dn=             1 << 25,
  44         Fz=             1 << 24,
  45         /* trap exception enables (not allowed in vfp3) */
  46         FPIDNRM =       1 << 15,        /* input denormal */
  47         Alltraps = FPIDNRM | FPINEX | FPUNFL | FPOVFL | FPZDIV | FPINVAL,
  48         /* pending exceptions */
  49         FPAIDNRM =      1 << 7,         /* input denormal */
  50         Allexc = FPAIDNRM | FPAINEX | FPAUNFL | FPAOVFL | FPAZDIV | FPAINVAL,
  51         /* condition codes */
  52         Allcc =         MASK(4) << 28,
  53 };
  54 enum {
  55         /* CpCPaccess bits */
  56         Cpaccnosimd =   1u << 31,
  57         Cpaccd16 =      1 << 30,
  58 };
  59
  60 static char *
  61 subarch(int impl, uint sa)
  62 {
  63         static char *armarchs[] = {
  64                 "VFPv1 (unsupported)",
  65                 "VFPv2",
  66                 "VFPv3+ with common VFP subarch v2",
  67                 "VFPv3+ with null subarch",
  68                 "VFPv3+ with common VFP subarch v3",
  69         };
  70
  71         if (impl != 'A' || sa >= nelem(armarchs))
  72                 return "GOK";
  73         else
  74                 return armarchs[sa];
  75 }
  76
  77 static char *
  78 implement(uchar impl)
  79 {
  80         if (impl == 'A')
  81                 return "arm";
  82         else
  83                 return "unknown";
  84 }
  85
  86 static int
  87 havefp(void)
  88 {
  89         int gotfp;
  90         ulong acc, sid;
  91
  92         if (m->havefpvalid)
  93                 return m->havefp;
  94
  95         m->havefp = 0;
  96         gotfp = 1 << CpFP | 1 << CpDFP;
  97         cpwrsc(0, CpCONTROL, 0, CpCPaccess, MASK(28));
  98         acc = cprdsc(0, CpCONTROL, 0, CpCPaccess);
  99         if ((acc & (MASK(2) << (2*CpFP))) == 0) {
 100                 gotfp &= ~(1 << CpFP);
 101                 print("fpon: no single FP coprocessor\n");
 102         }
 103         if ((acc & (MASK(2) << (2*CpDFP))) == 0) {
 104                 gotfp &= ~(1 << CpDFP);
 105                 print("fpon: no double FP coprocessor\n");
 106         }
 107         if (!gotfp) {
 108                 print("fpon: no FP coprocessors\n");
 109                 m->havefpvalid = 1;
 110                 return 0;
 111         }
 112         m->fpon = 1;                    /* don't panic */
 113         sid = fprd(Fpsid);
 114         m->fpon = 0;
 115         switch((sid >> 16) & MASK(7)){
 116         case 0:                         /* VFPv1 */
 117                 break;
 118         case 1:                         /* VFPv2 */
 119                 m->havefp = VFPv2;
 120                 m->fpnregs = 16;
 121                 break;
 122         default:                        /* VFPv3 or later */
 123                 m->havefp = VFPv3;
 124                 m->fpnregs = (acc & Cpaccd16) ? 16 : 32;
 125                 break;
 126         }
 127         if (m->machno == 0)
 128                 print("fp: %d registers, %s simd\n", m->fpnregs,
 129                         (acc & Cpaccnosimd? " no": ""));
 130         m->havefpvalid = 1;
 131         return 1;
 132 }
 133
 134 /*
 135  * these can be called to turn the fpu on or off for user procs,
 136  * not just at system start up or shutdown.
 137  */
 138
 139 void
 140 fpoff(void)
 141 {
 142         if (m->fpon) {
 143                 fpwr(Fpexc, 0);
 144                 m->fpon = 0;
 145         }
 146 }
 147
 148 void
 149 fpononly(void)
 150 {
 151         if (!m->fpon && havefp()) {
 152                 /* enable fp.  must be first operation on the FPUs. */
 153                 fpwr(Fpexc, Fpenabled);
 154                 m->fpon = 1;
 155         }
 156 }
 157
 158 static void
 159 fpcfg(void)
 160 {
 161         int impl;
 162         ulong sid;
 163         static int printed;
 164
 165         /* clear pending exceptions; no traps in vfp3; all v7 ops are scalar */
 166         m->fpscr = Dn | Fz | FPRNR | (FPINVAL | FPZDIV | FPOVFL) & ~Alltraps;
 167         fpwr(Fpscr, m->fpscr);
 168         m->fpconfiged = 1;
 169
 170         if (printed)
 171                 return;
 172         sid = fprd(Fpsid);
 173         impl = sid >> 24;
 174         print("fp: %s arch %s; rev %ld\n", implement(impl),
 175                 subarch(impl, (sid >> 16) & MASK(7)), sid & MASK(4));
 176         printed = 1;
 177 }
 178
 179 void
 180 fpinit(void)
 181 {
 182         if (havefp()) {
 183                 fpononly();
 184                 fpcfg();
 185         }
 186 }
 187
 188 void
 189 fpon(void)
 190 {
 191         if (havefp()) {
 192                 fpononly();
 193                 if (m->fpconfiged)
 194                         fpwr(Fpscr, (fprd(Fpscr) & Allcc) | m->fpscr);
 195                 else
 196                         fpcfg();        /* 1st time on this fpu; configure it */
 197         }
 198 }
 199
 200 void
 201 fpclear(void)
 202 {
 203 //      ulong scr;
 204
 205         fpon();
 206 //      scr = fprd(Fpscr);
 207 //      m->fpscr = scr & ~Allexc;
 208 //      fpwr(Fpscr, m->fpscr);
 209
 210         fpwr(Fpexc, fprd(Fpexc) & ~Fpmbc);
 211 }
 212
 213
 214 /*
 215  * Called when a note is about to be delivered to a
 216  * user process, usually at the end of a system call.
 217  * Note handlers are not allowed to use the FPU so
 218  * the state is marked (after saving if necessary) and
 219  * checked in the Device Not Available handler.
 220  */
 221 void
 222 fpunotify(Ureg*)
 223 {
 224         if(up->fpstate == FPactive){
 225                 fpsave(up->fpsave);
 226                 up->fpstate = FPinactive;
 227         }
 228         up->fpstate |= FPillegal;
 229 }
 230
 231 /*
 232  * Called from sysnoted() via the machine-dependent
 233  * noted() routine.
 234  * Clear the flag set above in fpunotify().
 235  */
 236 void
 237 fpunoted(void)
 238 {
 239         up->fpstate &= ~FPillegal;
 240 }
 241
 242 /* should only be called if p->fpstate == FPactive */
 243 void
 244 fpsave(FPsave *fps)
 245 {
 246         int n;
 247
 248         fpon();
 249         fps->control = fps->status = fprd(Fpscr);
 250         assert(m->fpnregs);
 251         for (n = 0; n < m->fpnregs; n++)
 252                 fpsavereg(n, (uvlong *)fps->regs[n]);
 253         fpoff();
 254 }
 255
 256 static void
 257 fprestore(Proc *p)
 258 {
 259         int n;
 260
 261         fpon();
 262         fpwr(Fpscr, p->fpsave->control);
 263         m->fpscr = fprd(Fpscr) & ~Allcc;
 264         assert(m->fpnregs);
 265         for (n = 0; n < m->fpnregs; n++)
 266                 fprestreg(n, *(uvlong *)p->fpsave->regs[n]);
 267 }
 268
 269 /*
 270  * Called from sched() and sleep() via the machine-dependent
 271  * procsave() routine.
 272  * About to go in to the scheduler.
 273  * If the process wasn't using the FPU
 274  * there's nothing to do.
 275  */
 276 void
 277 fpuprocsave(Proc *p)
 278 {
 279         if(p->fpstate == FPactive){
 280                 if(p->state == Moribund)
 281                         fpclear();
 282                 else{
 283                         /*
 284                          * Fpsave() stores without handling pending
 285                          * unmasked exeptions. Postnote() can't be called
 286                          * here as sleep() already has up->rlock, so
 287                          * the handling of pending exceptions is delayed
 288                          * until the process runs again and generates an
 289                          * emulation fault to activate the FPU.
 290                          */
 291                         fpsave(p->fpsave);
 292                 }
 293                 p->fpstate = FPinactive;
 294         }
 295 }
 296
 297 /*
 298  * The process has been rescheduled and is about to run.
 299  * Nothing to do here right now. If the process tries to use
 300  * the FPU again it will cause a Device Not Available
 301  * exception and the state will then be restored.
 302  */
 303 void
 304 fpuprocrestore(Proc *)
 305 {
 306 }
 307
 308 /*
 309  * The current process has been forked,
 310  * save and copy neccesary state to child.
 311  */
 312 void
 313 fpuprocfork(Proc *p)
 314 {
 315         int s;
 316
 317         s = splhi();
 318         switch(up->fpstate & ~FPillegal){
 319         case FPactive:
 320                 fpsave(up->fpsave);
 321                 up->fpstate = FPinactive;
 322                 /* no break */
 323         case FPinactive:
 324                 memmove(p->fpsave, up->fpsave, sizeof(FPsave));
 325                 p->fpstate = FPinactive;
 326         }
 327         splx(s);
 328 }
 329
 330 /*
 331  * Disable the FPU.
 332  * Called from sysexec() via sysprocsetup() to
 333  * set the FPU for the new process.
 334  */
 335 void
 336 fpusysprocsetup(Proc *p)
 337 {
 338         p->fpstate = FPinit;
 339         fpoff();
 340 }
 341
 342 static void
 343 mathnote(void)
 344 {
 345         ulong status;
 346         char *msg, note[ERRMAX];
 347
 348         status = up->fpsave->status;
 349
 350         /*
 351          * Some attention should probably be paid here to the
 352          * exception masks and error summary.
 353          */
 354         if (status & FPAINEX)
 355                 msg = "inexact";
 356         else if (status & FPAOVFL)
 357                 msg = "overflow";
 358         else if (status & FPAUNFL)
 359                 msg = "underflow";
 360         else if (status & FPAZDIV)
 361                 msg = "divide by zero";
 362         else if (status & FPAINVAL)
 363                 msg = "bad operation";
 364         else
 365                 msg = "spurious";
 366         snprint(note, sizeof note, "sys: fp: %s fppc=%#p status=%#lux",
 367                 msg, up->fpsave->pc, status);
 368         postnote(up, 1, note, NDebug);
 369 }
 370
 371 static void
 372 mathemu(Ureg *)
 373 {
 374         if(m->havefp == VFPv3 && !(fprd(Fpexc) & (Fpex|Fpdex)))
 375                 iprint("mathemu: not an FP exception but an unknown FP opcode\n");
 376         switch(up->fpstate){
 377         case FPemu:
 378                 error("illegal instruction: VFP opcode in emulated mode");
 379         case FPinit:
 380                 fpinit();
 381                 up->fpstate = FPactive;
 382                 break;
 383         case FPinactive:
 384                 /*
 385                  * Before restoring the state, check for any pending
 386                  * exceptions.  There's no way to restore the state without
 387                  * generating an unmasked exception.
 388                  * More attention should probably be paid here to the
 389                  * exception masks and error summary.
 390                  */
 391                 if(up->fpsave->status & (FPAINEX|FPAUNFL|FPAOVFL|FPAZDIV|FPAINVAL)){
 392                         mathnote();
 393                         break;
 394                 }
 395                 fprestore(up);
 396                 up->fpstate = FPactive;
 397                 break;
 398         case FPactive:
 399                 error("illegal instruction: bad vfp fpu opcode");
 400                 break;
 401         }
 402         fpclear();
 403 }
 404
 405 void
 406 fpstuck(uintptr pc)
 407 {
 408         if (m->fppc == pc && m->fppid == up->pid) {
 409                 m->fpcnt++;
 410                 if (m->fpcnt > 4)
 411                         panic("fpuemu: cpu%d stuck at pid %ld %s pc %#p "
 412                                 "instr %#8.8lux", m->machno, up->pid, up->text,
 413                                 pc, *(ulong *)pc);
 414         } else {
 415                 m->fppid = up->pid;
 416                 m->fppc = pc;
 417                 m->fpcnt = 0;
 418         }
 419 }
 420
 421 enum {
 422         N = 1<<31,
 423         Z = 1<<30,
 424         C = 1<<29,
 425         V = 1<<28,
 426         REGPC = 15,
 427 };
 428
 429 static int
 430 condok(int cc, int c)
 431 {
 432         switch(c){
 433         case 0: /* Z set */
 434                 return cc&Z;
 435         case 1: /* Z clear */
 436                 return (cc&Z) == 0;
 437         case 2: /* C set */
 438                 return cc&C;
 439         case 3: /* C clear */
 440                 return (cc&C) == 0;
 441         case 4: /* N set */
 442                 return cc&N;
 443         case 5: /* N clear */
 444                 return (cc&N) == 0;
 445         case 6: /* V set */
 446                 return cc&V;
 447         case 7: /* V clear */
 448                 return (cc&V) == 0;
 449         case 8: /* C set and Z clear */
 450                 return cc&C && (cc&Z) == 0;
 451         case 9: /* C clear or Z set */
 452                 return (cc&C) == 0 || cc&Z;
 453         case 10:        /* N set and V set, or N clear and V clear */
 454                 return (~cc&(N|V))==0 || (cc&(N|V)) == 0;
 455         case 11:        /* N set and V clear, or N clear and V set */
 456                 return (cc&(N|V))==N || (cc&(N|V))==V;
 457         case 12:        /* Z clear, and either N set and V set or N clear and V clear */
 458                 return (cc&Z) == 0 && ((~cc&(N|V))==0 || (cc&(N|V))==0);
 459         case 13:        /* Z set, or N set and V clear or N clear and V set */
 460                 return (cc&Z) || (cc&(N|V))==N || (cc&(N|V))==V;
 461         case 14:        /* always */
 462                 return 1;
 463         case 15:        /* never (reserved) */
 464                 return 0;
 465         }
 466         return 0;       /* not reached */
 467 }
 468
 469 /* only called to deal with user-mode instruction faults */
 470 int
 471 fpuemu(Ureg* ureg)
 472 {
 473         int s, nfp, cop, op;
 474         uintptr pc;
 475
 476         if(waserror()){
 477                 postnote(up, 1, up->errstr, NDebug);
 478                 return 1;
 479         }
 480
 481         if(up->fpstate & FPillegal)
 482                 error("floating point in note handler");
 483
 484         nfp = 0;
 485         pc = ureg->pc;
 486         validaddr(pc, 4, 0);
 487         if(!condok(ureg->psr, *(ulong*)pc >> 28))
 488                 iprint("fpuemu: conditional instr shouldn't have got here\n");
 489         op  = (*(ulong *)pc >> 24) & MASK(4);
 490         cop = (*(ulong *)pc >>  8) & MASK(4);
 491         if(m->fpon)
 492                 fpstuck(pc);            /* debugging; could move down 1 line */
 493         if (ISFPAOP(cop, op)) {         /* old arm 7500 fpa opcode? */
 494 //              iprint("fpuemu: fpa instr %#8.8lux at %#p\n", *(ulong *)pc, pc);
 495 //              error("illegal instruction: old arm 7500 fpa opcode");
 496                 s = spllo();
 497                 if(waserror()){
 498                         splx(s);
 499                         nexterror();
 500                 }
 501                 nfp = fpiarm(ureg);     /* advances pc past emulated instr(s) */
 502                 if (nfp > 1)            /* could adjust this threshold */
 503                         m->fppc = m->fpcnt = 0;
 504                 splx(s);
 505                 poperror();
 506         } else if (ISVFPOP(cop, op)) {  /* if vfp, fpu must be off */
 507                 mathemu(ureg);          /* enable fpu & retry */
 508                 nfp = 1;
 509         }
 510
 511         poperror();
 512         return nfp;
 513 }