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