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