math.c 9.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506
  1. /*
  2. * arch/sh/math-emu/math.c
  3. *
  4. * Copyright (C) 2006 Takashi YOSHII <takasi-y@ops.dti.ne.jp>
  5. *
  6. * This file is subject to the terms and conditions of the GNU General Public
  7. * License. See the file "COPYING" in the main directory of this archive
  8. * for more details.
  9. */
  10. #include <linux/kernel.h>
  11. #include <linux/errno.h>
  12. #include <linux/types.h>
  13. #include <linux/sched/signal.h>
  14. #include <linux/signal.h>
  15. #include <linux/perf_event.h>
  16. #include <linux/uaccess.h>
  17. #include <asm/processor.h>
  18. #include <asm/io.h>
  19. #include "sfp-util.h"
  20. #include <math-emu/soft-fp.h>
  21. #include <math-emu/single.h>
  22. #include <math-emu/double.h>
  23. #define FPUL (fregs->fpul)
  24. #define FPSCR (fregs->fpscr)
  25. #define FPSCR_RM (FPSCR&3)
  26. #define FPSCR_DN ((FPSCR>>18)&1)
  27. #define FPSCR_PR ((FPSCR>>19)&1)
  28. #define FPSCR_SZ ((FPSCR>>20)&1)
  29. #define FPSCR_FR ((FPSCR>>21)&1)
  30. #define FPSCR_MASK 0x003fffffUL
  31. #define BANK(n) (n^(FPSCR_FR?16:0))
  32. #define FR ((unsigned long*)(fregs->fp_regs))
  33. #define FR0 (FR[BANK(0)])
  34. #define FRn (FR[BANK(n)])
  35. #define FRm (FR[BANK(m)])
  36. #define DR ((unsigned long long*)(fregs->fp_regs))
  37. #define DRn (DR[BANK(n)/2])
  38. #define DRm (DR[BANK(m)/2])
  39. #define XREG(n) (n^16)
  40. #define XFn (FR[BANK(XREG(n))])
  41. #define XFm (FR[BANK(XREG(m))])
  42. #define XDn (DR[BANK(XREG(n))/2])
  43. #define XDm (DR[BANK(XREG(m))/2])
  44. #define R0 (regs->regs[0])
  45. #define Rn (regs->regs[n])
  46. #define Rm (regs->regs[m])
  47. #define WRITE(d,a) ({if(put_user(d, (typeof (d)*)a)) return -EFAULT;})
  48. #define READ(d,a) ({if(get_user(d, (typeof (d)*)a)) return -EFAULT;})
  49. #define PACK_S(r,f) FP_PACK_SP(&r,f)
  50. #define UNPACK_S(f,r) FP_UNPACK_SP(f,&r)
  51. #define PACK_D(r,f) \
  52. {u32 t[2]; FP_PACK_DP(t,f); ((u32*)&r)[0]=t[1]; ((u32*)&r)[1]=t[0];}
  53. #define UNPACK_D(f,r) \
  54. {u32 t[2]; t[0]=((u32*)&r)[1]; t[1]=((u32*)&r)[0]; FP_UNPACK_DP(f,t);}
  55. // 2 args instructions.
  56. #define BOTH_PRmn(op,x) \
  57. FP_DECL_EX; if(FPSCR_PR) op(D,x,DRm,DRn); else op(S,x,FRm,FRn);
  58. #define CMP_X(SZ,R,M,N) do{ \
  59. FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); \
  60. UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \
  61. FP_CMP_##SZ(R, Fn, Fm, 2); }while(0)
  62. #define EQ_X(SZ,R,M,N) do{ \
  63. FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); \
  64. UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \
  65. FP_CMP_EQ_##SZ(R, Fn, Fm); }while(0)
  66. #define CMP(OP) ({ int r; BOTH_PRmn(OP##_X,r); r; })
  67. static int
  68. fcmp_gt(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  69. {
  70. if (CMP(CMP) > 0)
  71. regs->sr |= 1;
  72. else
  73. regs->sr &= ~1;
  74. return 0;
  75. }
  76. static int
  77. fcmp_eq(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  78. {
  79. if (CMP(CMP /*EQ*/) == 0)
  80. regs->sr |= 1;
  81. else
  82. regs->sr &= ~1;
  83. return 0;
  84. }
  85. #define ARITH_X(SZ,OP,M,N) do{ \
  86. FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); FP_DECL_##SZ(Fr); \
  87. UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \
  88. FP_##OP##_##SZ(Fr, Fn, Fm); \
  89. PACK_##SZ(N, Fr); }while(0)
  90. static int
  91. fadd(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  92. {
  93. BOTH_PRmn(ARITH_X, ADD);
  94. return 0;
  95. }
  96. static int
  97. fsub(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  98. {
  99. BOTH_PRmn(ARITH_X, SUB);
  100. return 0;
  101. }
  102. static int
  103. fmul(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  104. {
  105. BOTH_PRmn(ARITH_X, MUL);
  106. return 0;
  107. }
  108. static int
  109. fdiv(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  110. {
  111. BOTH_PRmn(ARITH_X, DIV);
  112. return 0;
  113. }
  114. static int
  115. fmac(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  116. {
  117. FP_DECL_EX;
  118. FP_DECL_S(Fr);
  119. FP_DECL_S(Ft);
  120. FP_DECL_S(F0);
  121. FP_DECL_S(Fm);
  122. FP_DECL_S(Fn);
  123. UNPACK_S(F0, FR0);
  124. UNPACK_S(Fm, FRm);
  125. UNPACK_S(Fn, FRn);
  126. FP_MUL_S(Ft, Fm, F0);
  127. FP_ADD_S(Fr, Fn, Ft);
  128. PACK_S(FRn, Fr);
  129. return 0;
  130. }
  131. // to process fmov's extension (odd n for DR access XD).
  132. #define FMOV_EXT(x) if(x&1) x+=16-1
  133. static int
  134. fmov_idx_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  135. int n)
  136. {
  137. if (FPSCR_SZ) {
  138. FMOV_EXT(n);
  139. READ(FRn, Rm + R0 + 4);
  140. n++;
  141. READ(FRn, Rm + R0);
  142. } else {
  143. READ(FRn, Rm + R0);
  144. }
  145. return 0;
  146. }
  147. static int
  148. fmov_mem_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  149. int n)
  150. {
  151. if (FPSCR_SZ) {
  152. FMOV_EXT(n);
  153. READ(FRn, Rm + 4);
  154. n++;
  155. READ(FRn, Rm);
  156. } else {
  157. READ(FRn, Rm);
  158. }
  159. return 0;
  160. }
  161. static int
  162. fmov_inc_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  163. int n)
  164. {
  165. if (FPSCR_SZ) {
  166. FMOV_EXT(n);
  167. READ(FRn, Rm + 4);
  168. n++;
  169. READ(FRn, Rm);
  170. Rm += 8;
  171. } else {
  172. READ(FRn, Rm);
  173. Rm += 4;
  174. }
  175. return 0;
  176. }
  177. static int
  178. fmov_reg_idx(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  179. int n)
  180. {
  181. if (FPSCR_SZ) {
  182. FMOV_EXT(m);
  183. WRITE(FRm, Rn + R0 + 4);
  184. m++;
  185. WRITE(FRm, Rn + R0);
  186. } else {
  187. WRITE(FRm, Rn + R0);
  188. }
  189. return 0;
  190. }
  191. static int
  192. fmov_reg_mem(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  193. int n)
  194. {
  195. if (FPSCR_SZ) {
  196. FMOV_EXT(m);
  197. WRITE(FRm, Rn + 4);
  198. m++;
  199. WRITE(FRm, Rn);
  200. } else {
  201. WRITE(FRm, Rn);
  202. }
  203. return 0;
  204. }
  205. static int
  206. fmov_reg_dec(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  207. int n)
  208. {
  209. if (FPSCR_SZ) {
  210. FMOV_EXT(m);
  211. Rn -= 8;
  212. WRITE(FRm, Rn + 4);
  213. m++;
  214. WRITE(FRm, Rn);
  215. } else {
  216. Rn -= 4;
  217. WRITE(FRm, Rn);
  218. }
  219. return 0;
  220. }
  221. static int
  222. fmov_reg_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
  223. int n)
  224. {
  225. if (FPSCR_SZ) {
  226. FMOV_EXT(m);
  227. FMOV_EXT(n);
  228. DRn = DRm;
  229. } else {
  230. FRn = FRm;
  231. }
  232. return 0;
  233. }
  234. static int
  235. fnop_mn(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  236. {
  237. return -EINVAL;
  238. }
  239. // 1 arg instructions.
  240. #define NOTYETn(i) static int i(struct sh_fpu_soft_struct *fregs, int n) \
  241. { printk( #i " not yet done.\n"); return 0; }
  242. NOTYETn(ftrv)
  243. NOTYETn(fsqrt)
  244. NOTYETn(fipr)
  245. NOTYETn(fsca)
  246. NOTYETn(fsrra)
  247. #define EMU_FLOAT_X(SZ,N) do { \
  248. FP_DECL_##SZ(Fn); \
  249. FP_FROM_INT_##SZ(Fn, FPUL, 32, int); \
  250. PACK_##SZ(N, Fn); }while(0)
  251. static int ffloat(struct sh_fpu_soft_struct *fregs, int n)
  252. {
  253. FP_DECL_EX;
  254. if (FPSCR_PR)
  255. EMU_FLOAT_X(D, DRn);
  256. else
  257. EMU_FLOAT_X(S, FRn);
  258. return 0;
  259. }
  260. #define EMU_FTRC_X(SZ,N) do { \
  261. FP_DECL_##SZ(Fn); \
  262. UNPACK_##SZ(Fn, N); \
  263. FP_TO_INT_##SZ(FPUL, Fn, 32, 1); }while(0)
  264. static int ftrc(struct sh_fpu_soft_struct *fregs, int n)
  265. {
  266. FP_DECL_EX;
  267. if (FPSCR_PR)
  268. EMU_FTRC_X(D, DRn);
  269. else
  270. EMU_FTRC_X(S, FRn);
  271. return 0;
  272. }
  273. static int fcnvsd(struct sh_fpu_soft_struct *fregs, int n)
  274. {
  275. FP_DECL_EX;
  276. FP_DECL_S(Fn);
  277. FP_DECL_D(Fr);
  278. UNPACK_S(Fn, FPUL);
  279. FP_CONV(D, S, 2, 1, Fr, Fn);
  280. PACK_D(DRn, Fr);
  281. return 0;
  282. }
  283. static int fcnvds(struct sh_fpu_soft_struct *fregs, int n)
  284. {
  285. FP_DECL_EX;
  286. FP_DECL_D(Fn);
  287. FP_DECL_S(Fr);
  288. UNPACK_D(Fn, DRn);
  289. FP_CONV(S, D, 1, 2, Fr, Fn);
  290. PACK_S(FPUL, Fr);
  291. return 0;
  292. }
  293. static int fxchg(struct sh_fpu_soft_struct *fregs, int flag)
  294. {
  295. FPSCR ^= flag;
  296. return 0;
  297. }
  298. static int fsts(struct sh_fpu_soft_struct *fregs, int n)
  299. {
  300. FRn = FPUL;
  301. return 0;
  302. }
  303. static int flds(struct sh_fpu_soft_struct *fregs, int n)
  304. {
  305. FPUL = FRn;
  306. return 0;
  307. }
  308. static int fneg(struct sh_fpu_soft_struct *fregs, int n)
  309. {
  310. FRn ^= (1 << (_FP_W_TYPE_SIZE - 1));
  311. return 0;
  312. }
  313. static int fabs(struct sh_fpu_soft_struct *fregs, int n)
  314. {
  315. FRn &= ~(1 << (_FP_W_TYPE_SIZE - 1));
  316. return 0;
  317. }
  318. static int fld0(struct sh_fpu_soft_struct *fregs, int n)
  319. {
  320. FRn = 0;
  321. return 0;
  322. }
  323. static int fld1(struct sh_fpu_soft_struct *fregs, int n)
  324. {
  325. FRn = (_FP_EXPBIAS_S << (_FP_FRACBITS_S - 1));
  326. return 0;
  327. }
  328. static int fnop_n(struct sh_fpu_soft_struct *fregs, int n)
  329. {
  330. return -EINVAL;
  331. }
  332. /// Instruction decoders.
  333. static int id_fxfd(struct sh_fpu_soft_struct *, int);
  334. static int id_fnxd(struct sh_fpu_soft_struct *, struct pt_regs *, int, int);
  335. static int (*fnxd[])(struct sh_fpu_soft_struct *, int) = {
  336. fsts, flds, ffloat, ftrc, fneg, fabs, fsqrt, fsrra,
  337. fld0, fld1, fcnvsd, fcnvds, fnop_n, fnop_n, fipr, id_fxfd
  338. };
  339. static int (*fnmx[])(struct sh_fpu_soft_struct *, struct pt_regs *, int, int) = {
  340. fadd, fsub, fmul, fdiv, fcmp_eq, fcmp_gt, fmov_idx_reg, fmov_reg_idx,
  341. fmov_mem_reg, fmov_inc_reg, fmov_reg_mem, fmov_reg_dec,
  342. fmov_reg_reg, id_fnxd, fmac, fnop_mn};
  343. static int id_fxfd(struct sh_fpu_soft_struct *fregs, int x)
  344. {
  345. const int flag[] = { FPSCR_SZ, FPSCR_PR, FPSCR_FR, 0 };
  346. switch (x & 3) {
  347. case 3:
  348. fxchg(fregs, flag[x >> 2]);
  349. break;
  350. case 1:
  351. ftrv(fregs, x - 1);
  352. break;
  353. default:
  354. fsca(fregs, x);
  355. }
  356. return 0;
  357. }
  358. static int
  359. id_fnxd(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int x, int n)
  360. {
  361. return (fnxd[x])(fregs, n);
  362. }
  363. static int
  364. id_fnmx(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, u16 code)
  365. {
  366. int n = (code >> 8) & 0xf, m = (code >> 4) & 0xf, x = code & 0xf;
  367. return (fnmx[x])(fregs, regs, m, n);
  368. }
  369. static int
  370. id_sys(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, u16 code)
  371. {
  372. int n = ((code >> 8) & 0xf);
  373. unsigned long *reg = (code & 0x0010) ? &FPUL : &FPSCR;
  374. switch (code & 0xf0ff) {
  375. case 0x005a:
  376. case 0x006a:
  377. Rn = *reg;
  378. break;
  379. case 0x405a:
  380. case 0x406a:
  381. *reg = Rn;
  382. break;
  383. case 0x4052:
  384. case 0x4062:
  385. Rn -= 4;
  386. WRITE(*reg, Rn);
  387. break;
  388. case 0x4056:
  389. case 0x4066:
  390. READ(*reg, Rn);
  391. Rn += 4;
  392. break;
  393. default:
  394. return -EINVAL;
  395. }
  396. return 0;
  397. }
  398. static int fpu_emulate(u16 code, struct sh_fpu_soft_struct *fregs, struct pt_regs *regs)
  399. {
  400. if ((code & 0xf000) == 0xf000)
  401. return id_fnmx(fregs, regs, code);
  402. else
  403. return id_sys(fregs, regs, code);
  404. }
  405. /**
  406. * fpu_init - Initialize FPU registers
  407. * @fpu: Pointer to software emulated FPU registers.
  408. */
  409. static void fpu_init(struct sh_fpu_soft_struct *fpu)
  410. {
  411. int i;
  412. fpu->fpscr = FPSCR_INIT;
  413. fpu->fpul = 0;
  414. for (i = 0; i < 16; i++) {
  415. fpu->fp_regs[i] = 0;
  416. fpu->xfp_regs[i]= 0;
  417. }
  418. }
  419. /**
  420. * do_fpu_inst - Handle reserved instructions for FPU emulation
  421. * @inst: instruction code.
  422. * @regs: registers on stack.
  423. */
  424. int do_fpu_inst(unsigned short inst, struct pt_regs *regs)
  425. {
  426. struct task_struct *tsk = current;
  427. struct sh_fpu_soft_struct *fpu = &(tsk->thread.xstate->softfpu);
  428. perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, 0);
  429. if (!(task_thread_info(tsk)->status & TS_USEDFPU)) {
  430. /* initialize once. */
  431. fpu_init(fpu);
  432. task_thread_info(tsk)->status |= TS_USEDFPU;
  433. }
  434. return fpu_emulate(inst, fpu, regs);
  435. }