sys/src/9/teg2/coproc.c

   1 /*
   2  * arm co-processors
   3  * mainly to cope with arm hard-wiring register numbers into instructions.
   4  *
   5  * CP15 (system control) is the one that gets used the most in practice.
   6  * these routines must be callable from KZERO space or the 0 segment.
   7  */
   8 #include "u.h"
   9 #include "../port/lib.h"
  10 #include "mem.h"
  11 #include "dat.h"
  12 #include "fns.h"
  13 #include "io.h"
  14
  15 #include "arm.h"
  16
  17 enum {
  18         /* alternates:  0xe12fff1e      BX (R14); last e is R14 */
  19         /*              0xe28ef000      B 0(R14); second e is R14 (ken) */
  20         Retinst = 0xe1a0f00e,           /* MOV R14, R15 */
  21
  22         Opmask  = MASK(3),
  23         Regmask = MASK(4),
  24 };
  25
  26 typedef ulong (*Pufv)(void);
  27 typedef void  (*Pvfu)(ulong);
  28
  29 static void
  30 setupcpop(ulong instr[2], ulong opcode, int cp, int op1, int crn, int crm,
  31         int op2)
  32 {
  33         ulong instrsz[2];
  34
  35         op1 &= Opmask;
  36         op2 &= Opmask;
  37         crn &= Regmask;
  38         crm &= Regmask;
  39         cp  &= Regmask;
  40         instr[0] = opcode | op1 << 21 | crn << 16 | cp << 8 | op2 << 5 | crm;
  41         instr[1] = Retinst;
  42
  43         cachedwbse(instr, sizeof instrsz);
  44         cacheiinv();
  45 }
  46
  47 ulong
  48 cprd(int cp, int op1, int crn, int crm, int op2)
  49 {
  50         int s, r;
  51         volatile ulong instr[2];
  52         Pufv fp;
  53
  54         s = splhi();
  55         /*
  56          * MRC.  return value will be in R0, which is convenient.
  57          * Rt will be R0.
  58          */
  59         setupcpop(instr, 0xee100010, cp, op1, crn, crm, op2);
  60         fp = (Pufv)instr;
  61         r = fp();
  62         splx(s);
  63         return r;
  64 }
  65
  66 void
  67 cpwr(int cp, int op1, int crn, int crm, int op2, ulong val)
  68 {
  69         int s;
  70         volatile ulong instr[2];
  71         Pvfu fp;
  72
  73         s = splhi();
  74         setupcpop(instr, 0xee000010, cp, op1, crn, crm, op2); /* MCR, Rt is R0 */
  75         fp = (Pvfu)instr;
  76         fp(val);
  77         coherence();
  78         splx(s);
  79 }
  80
  81 ulong
  82 cprdsc(int op1, int crn, int crm, int op2)
  83 {
  84         return cprd(CpSC, op1, crn, crm, op2);
  85 }
  86
  87 void
  88 cpwrsc(int op1, int crn, int crm, int op2, ulong val)
  89 {
  90         cpwr(CpSC, op1, crn, crm, op2, val);
  91 }
  92
  93 /* floating point */
  94
  95 /* fp coproc control */
  96 static void
  97 setupfpctlop(ulong instr[2], int opcode, int fpctlreg)
  98 {
  99         ulong instrsz[2];
 100
 101         fpctlreg &= Nfpctlregs - 1;
 102         instr[0] = opcode | fpctlreg << 16 | 0 << 12 | CpFP << 8;
 103         instr[1] = Retinst;
 104
 105         cachedwbse(instr, sizeof instrsz);
 106         cacheiinv();
 107 }
 108
 109 ulong
 110 fprd(int fpreg)
 111 {
 112         int s, r;
 113         volatile ulong instr[2];
 114         Pufv fp;
 115
 116         if (!m->fpon) {
 117                 dumpstack();
 118                 panic("fprd: cpu%d fpu off", m->machno);
 119         }
 120         s = splhi();
 121         /*
 122          * VMRS.  return value will be in R0, which is convenient.
 123          * Rt will be R0.
 124          */
 125         setupfpctlop(instr, 0xeef00010, fpreg);
 126         fp = (Pufv)instr;
 127         r = fp();
 128         splx(s);
 129         return r;
 130 }
 131
 132 void
 133 fpwr(int fpreg, ulong val)
 134 {
 135         int s;
 136         volatile ulong instr[2];
 137         Pvfu fp;
 138
 139         /* fpu might be off and this VMSR might enable it */
 140         s = splhi();
 141         setupfpctlop(instr, 0xeee00010, fpreg);         /* VMSR, Rt is R0 */
 142         fp = (Pvfu)instr;
 143         fp(val);
 144         coherence();
 145         splx(s);
 146 }
 147
 148 /* fp register access; don't bother with single precision */
 149 static void
 150 setupfpop(ulong instr[2], int opcode, int fpreg)
 151 {
 152         ulong instrsz[2];
 153
 154         instr[0] = opcode | 0 << 16 | (fpreg & (16 - 1)) << 12;
 155         if (fpreg >= 16)
 156                 instr[0] |= 1 << 22;            /* high bit of dfp reg # */
 157         instr[1] = Retinst;
 158
 159         cachedwbse(instr, sizeof instrsz);
 160         cacheiinv();
 161 }
 162
 163 ulong
 164 fpsavereg(int fpreg, uvlong *fpp)
 165 {
 166         int s, r;
 167         volatile ulong instr[2];
 168         ulong (*fp)(uvlong *);
 169
 170         if (!m->fpon)
 171                 panic("fpsavereg: cpu%d fpu off", m->machno);
 172         s = splhi();
 173         /*
 174          * VSTR.  pointer will be in R0, which is convenient.
 175          * Rt will be R0.
 176          */
 177         setupfpop(instr, 0xed000000 | CpDFP << 8, fpreg);
 178         fp = (ulong (*)(uvlong *))instr;
 179         r = fp(fpp);
 180         splx(s);
 181         coherence();
 182         return r;                       /* not too meaningful */
 183 }
 184
 185 void
 186 fprestreg(int fpreg, uvlong val)
 187 {
 188         int s;
 189         volatile ulong instr[2];
 190         void (*fp)(uvlong *);
 191
 192         if (!m->fpon)
 193                 panic("fprestreg: cpu%d fpu off", m->machno);
 194         s = splhi();
 195         setupfpop(instr, 0xed100000 | CpDFP << 8, fpreg); /* VLDR, Rt is R0 */
 196         fp = (void (*)(uvlong *))instr;
 197         fp(&val);
 198         coherence();
 199         splx(s);
 200 }