1/* 2 * align.c - address exception handler for M32R 3 * 4 * Copyright (c) 2003 Hitoshi Yamamoto 5 */ 6 7#include <asm/ptrace.h> 8#include <asm/uaccess.h> 9 10static int get_reg(struct pt_regs *regs, int nr) 11{ 12 int val; 13 14 if (nr < 4) 15 val = *(unsigned long *)(®s->r0 + nr); 16 else if (nr < 7) 17 val = *(unsigned long *)(®s->r4 + (nr - 4)); 18 else if (nr < 13) 19 val = *(unsigned long *)(®s->r7 + (nr - 7)); 20 else 21 val = *(unsigned long *)(®s->fp + (nr - 13)); 22 23 return val; 24} 25 26static void set_reg(struct pt_regs *regs, int nr, int val) 27{ 28 if (nr < 4) 29 *(unsigned long *)(®s->r0 + nr) = val; 30 else if (nr < 7) 31 *(unsigned long *)(®s->r4 + (nr - 4)) = val; 32 else if (nr < 13) 33 *(unsigned long *)(®s->r7 + (nr - 7)) = val; 34 else 35 *(unsigned long *)(®s->fp + (nr - 13)) = val; 36} 37 38#define REG1(insn) (((insn) & 0x0f00) >> 8) 39#define REG2(insn) ((insn) & 0x000f) 40#define PSW_BC 0x100 41 42/* O- instruction */ 43#define ISA_LD1 0x20c0 /* ld Rdest, @Rsrc */ 44#define ISA_LD2 0x20e0 /* ld Rdest, @Rsrc+ */ 45#define ISA_LDH 0x20a0 /* ldh Rdest, @Rsrc */ 46#define ISA_LDUH 0x20b0 /* lduh Rdest, @Rsrc */ 47#define ISA_ST1 0x2040 /* st Rsrc1, @Rsrc2 */ 48#define ISA_ST2 0x2060 /* st Rsrc1, @+Rsrc2 */ 49#define ISA_ST3 0x2070 /* st Rsrc1, @-Rsrc2 */ 50#define ISA_STH1 0x2020 /* sth Rsrc1, @Rsrc2 */ 51#define ISA_STH2 0x2030 /* sth Rsrc1, @Rsrc2+ */ 52 53#ifdef CONFIG_ISA_DUAL_ISSUE 54 55/* OS instruction */ 56#define ISA_ADD 0x00a0 /* add Rdest, Rsrc */ 57#define ISA_ADDI 0x4000 /* addi Rdest, #imm8 */ 58#define ISA_ADDX 0x0090 /* addx Rdest, Rsrc */ 59#define ISA_AND 0x00c0 /* and Rdest, Rsrc */ 60#define ISA_CMP 0x0040 /* cmp Rsrc1, Rsrc2 */ 61#define ISA_CMPEQ 0x0060 /* cmpeq Rsrc1, Rsrc2 */ 62#define ISA_CMPU 0x0050 /* cmpu Rsrc1, Rsrc2 */ 63#define ISA_CMPZ 0x0070 /* cmpz Rsrc */ 64#define ISA_LDI 0x6000 /* ldi Rdest, #imm8 */ 65#define ISA_MV 0x1080 /* mv Rdest, Rsrc */ 66#define ISA_NEG 0x0030 /* neg Rdest, Rsrc */ 67#define ISA_NOP 0x7000 /* nop */ 68#define ISA_NOT 0x00b0 /* not Rdest, Rsrc */ 69#define ISA_OR 0x00e0 /* or Rdest, Rsrc */ 70#define ISA_SUB 0x0020 /* sub Rdest, Rsrc */ 71#define ISA_SUBX 0x0010 /* subx Rdest, Rsrc */ 72#define ISA_XOR 0x00d0 /* xor Rdest, Rsrc */ 73 74/* -S instruction */ 75#define ISA_MUL 0x1060 /* mul Rdest, Rsrc */ 76#define ISA_MULLO_A0 0x3010 /* mullo Rsrc1, Rsrc2, A0 */ 77#define ISA_MULLO_A1 0x3090 /* mullo Rsrc1, Rsrc2, A1 */ 78#define ISA_MVFACMI_A0 0x50f2 /* mvfacmi Rdest, A0 */ 79#define ISA_MVFACMI_A1 0x50f6 /* mvfacmi Rdest, A1 */ 80 81static int emu_addi(unsigned short insn, struct pt_regs *regs) 82{ 83 char imm = (char)(insn & 0xff); 84 int dest = REG1(insn); 85 int val; 86 87 val = get_reg(regs, dest); 88 val += imm; 89 set_reg(regs, dest, val); 90 91 return 0; 92} 93 94static int emu_ldi(unsigned short insn, struct pt_regs *regs) 95{ 96 char imm = (char)(insn & 0xff); 97 98 set_reg(regs, REG1(insn), (int)imm); 99 100 return 0; 101} 102 103static int emu_add(unsigned short insn, struct pt_regs *regs) 104{ 105 int dest = REG1(insn); 106 int src = REG2(insn); 107 int val; 108 109 val = get_reg(regs, dest); 110 val += get_reg(regs, src); 111 set_reg(regs, dest, val); 112 113 return 0; 114} 115 116static int emu_addx(unsigned short insn, struct pt_regs *regs) 117{ 118 int dest = REG1(insn); 119 unsigned int val, tmp; 120 121 val = regs->psw & PSW_BC ? 1 : 0; 122 tmp = get_reg(regs, dest); 123 val += tmp; 124 val += (unsigned int)get_reg(regs, REG2(insn)); 125 set_reg(regs, dest, val); 126 127 /* C bit set */ 128 if (val < tmp) 129 regs->psw |= PSW_BC; 130 else 131 regs->psw &= ~(PSW_BC); 132 133 return 0; 134} 135 136static int emu_and(unsigned short insn, struct pt_regs *regs) 137{ 138 int dest = REG1(insn); 139 int val; 140 141 val = get_reg(regs, dest); 142 val &= get_reg(regs, REG2(insn)); 143 set_reg(regs, dest, val); 144 145 return 0; 146} 147 148static int emu_cmp(unsigned short insn, struct pt_regs *regs) 149{ 150 if (get_reg(regs, REG1(insn)) < get_reg(regs, REG2(insn))) 151 regs->psw |= PSW_BC; 152 else 153 regs->psw &= ~(PSW_BC); 154 155 return 0; 156} 157 158static int emu_cmpeq(unsigned short insn, struct pt_regs *regs) 159{ 160 if (get_reg(regs, REG1(insn)) == get_reg(regs, REG2(insn))) 161 regs->psw |= PSW_BC; 162 else 163 regs->psw &= ~(PSW_BC); 164 165 return 0; 166} 167 168static int emu_cmpu(unsigned short insn, struct pt_regs *regs) 169{ 170 if ((unsigned int)get_reg(regs, REG1(insn)) 171 < (unsigned int)get_reg(regs, REG2(insn))) 172 regs->psw |= PSW_BC; 173 else 174 regs->psw &= ~(PSW_BC); 175 176 return 0; 177} 178 179static int emu_cmpz(unsigned short insn, struct pt_regs *regs) 180{ 181 if (!get_reg(regs, REG2(insn))) 182 regs->psw |= PSW_BC; 183 else 184 regs->psw &= ~(PSW_BC); 185 186 return 0; 187} 188 189static int emu_mv(unsigned short insn, struct pt_regs *regs) 190{ 191 int val; 192 193 val = get_reg(regs, REG2(insn)); 194 set_reg(regs, REG1(insn), val); 195 196 return 0; 197} 198 199static int emu_neg(unsigned short insn, struct pt_regs *regs) 200{ 201 int val; 202 203 val = get_reg(regs, REG2(insn)); 204 set_reg(regs, REG1(insn), 0 - val); 205 206 return 0; 207} 208 209static int emu_not(unsigned short insn, struct pt_regs *regs) 210{ 211 int val; 212 213 val = get_reg(regs, REG2(insn)); 214 set_reg(regs, REG1(insn), ~val); 215 216 return 0; 217} 218 219static int emu_or(unsigned short insn, struct pt_regs *regs) 220{ 221 int dest = REG1(insn); 222 int val; 223 224 val = get_reg(regs, dest); 225 val |= get_reg(regs, REG2(insn)); 226 set_reg(regs, dest, val); 227 228 return 0; 229} 230 231static int emu_sub(unsigned short insn, struct pt_regs *regs) 232{ 233 int dest = REG1(insn); 234 int val; 235 236 val = get_reg(regs, dest); 237 val -= get_reg(regs, REG2(insn)); 238 set_reg(regs, dest, val); 239 240 return 0; 241} 242 243static int emu_subx(unsigned short insn, struct pt_regs *regs) 244{ 245 int dest = REG1(insn); 246 unsigned int val, tmp; 247 248 val = tmp = get_reg(regs, dest); 249 val -= (unsigned int)get_reg(regs, REG2(insn)); 250 val -= regs->psw & PSW_BC ? 1 : 0; 251 set_reg(regs, dest, val); 252 253 /* C bit set */ 254 if (val > tmp) 255 regs->psw |= PSW_BC; 256 else 257 regs->psw &= ~(PSW_BC); 258 259 return 0; 260} 261 262static int emu_xor(unsigned short insn, struct pt_regs *regs) 263{ 264 int dest = REG1(insn); 265 unsigned int val; 266 267 val = (unsigned int)get_reg(regs, dest); 268 val ^= (unsigned int)get_reg(regs, REG2(insn)); 269 set_reg(regs, dest, val); 270 271 return 0; 272} 273 274static int emu_mul(unsigned short insn, struct pt_regs *regs) 275{ 276 int dest = REG1(insn); 277 int reg1, reg2; 278 279 reg1 = get_reg(regs, dest); 280 reg2 = get_reg(regs, REG2(insn)); 281 282 __asm__ __volatile__ ( 283 "mul %0, %1; \n\t" 284 : "+r" (reg1) : "r" (reg2) 285 ); 286 287 set_reg(regs, dest, reg1); 288 289 return 0; 290} 291 292static int emu_mullo_a0(unsigned short insn, struct pt_regs *regs) 293{ 294 int reg1, reg2; 295 296 reg1 = get_reg(regs, REG1(insn)); 297 reg2 = get_reg(regs, REG2(insn)); 298 299 __asm__ __volatile__ ( 300 "mullo %0, %1, a0; \n\t" 301 "mvfachi %0, a0; \n\t" 302 "mvfaclo %1, a0; \n\t" 303 : "+r" (reg1), "+r" (reg2) 304 ); 305 306 regs->acc0h = reg1; 307 regs->acc0l = reg2; 308 309 return 0; 310} 311 312static int emu_mullo_a1(unsigned short insn, struct pt_regs *regs) 313{ 314 int reg1, reg2; 315 316 reg1 = get_reg(regs, REG1(insn)); 317 reg2 = get_reg(regs, REG2(insn)); 318 319 __asm__ __volatile__ ( 320 "mullo %0, %1, a0; \n\t" 321 "mvfachi %0, a0; \n\t" 322 "mvfaclo %1, a0; \n\t" 323 : "+r" (reg1), "+r" (reg2) 324 ); 325 326 regs->acc1h = reg1; 327 regs->acc1l = reg2; 328 329 return 0; 330} 331 332static int emu_mvfacmi_a0(unsigned short insn, struct pt_regs *regs) 333{ 334 unsigned long val; 335 336 val = (regs->acc0h << 16) | (regs->acc0l >> 16); 337 set_reg(regs, REG1(insn), (int)val); 338 339 return 0; 340} 341 342static int emu_mvfacmi_a1(unsigned short insn, struct pt_regs *regs) 343{ 344 unsigned long val; 345 346 val = (regs->acc1h << 16) | (regs->acc1l >> 16); 347 set_reg(regs, REG1(insn), (int)val); 348 349 return 0; 350} 351 352static int emu_m32r2(unsigned short insn, struct pt_regs *regs) 353{ 354 int res = -1; 355 356 if ((insn & 0x7fff) == ISA_NOP) /* nop */ 357 return 0; 358 359 switch(insn & 0x7000) { 360 case ISA_ADDI: /* addi Rdest, #imm8 */ 361 res = emu_addi(insn, regs); 362 break; 363 case ISA_LDI: /* ldi Rdest, #imm8 */ 364 res = emu_ldi(insn, regs); 365 break; 366 default: 367 break; 368 } 369 370 if (!res) 371 return 0; 372 373 switch(insn & 0x70f0) { 374 case ISA_ADD: /* add Rdest, Rsrc */ 375 res = emu_add(insn, regs); 376 break; 377 case ISA_ADDX: /* addx Rdest, Rsrc */ 378 res = emu_addx(insn, regs); 379 break; 380 case ISA_AND: /* and Rdest, Rsrc */ 381 res = emu_and(insn, regs); 382 break; 383 case ISA_CMP: /* cmp Rsrc1, Rsrc2 */ 384 res = emu_cmp(insn, regs); 385 break; 386 case ISA_CMPEQ: /* cmpeq Rsrc1, Rsrc2 */ 387 res = emu_cmpeq(insn, regs); 388 break; 389 case ISA_CMPU: /* cmpu Rsrc1, Rsrc2 */ 390 res = emu_cmpu(insn, regs); 391 break; 392 case ISA_CMPZ: /* cmpz Rsrc */ 393 res = emu_cmpz(insn, regs); 394 break; 395 case ISA_MV: /* mv Rdest, Rsrc */ 396 res = emu_mv(insn, regs); 397 break; 398 case ISA_NEG: /* neg Rdest, Rsrc */ 399 res = emu_neg(insn, regs); 400 break; 401 case ISA_NOT: /* not Rdest, Rsrc */ 402 res = emu_not(insn, regs); 403 break; 404 case ISA_OR: /* or Rdest, Rsrc */ 405 res = emu_or(insn, regs); 406 break; 407 case ISA_SUB: /* sub Rdest, Rsrc */ 408 res = emu_sub(insn, regs); 409 break; 410 case ISA_SUBX: /* subx Rdest, Rsrc */ 411 res = emu_subx(insn, regs); 412 break; 413 case ISA_XOR: /* xor Rdest, Rsrc */ 414 res = emu_xor(insn, regs); 415 break; 416 case ISA_MUL: /* mul Rdest, Rsrc */ 417 res = emu_mul(insn, regs); 418 break; 419 case ISA_MULLO_A0: /* mullo Rsrc1, Rsrc2 */ 420 res = emu_mullo_a0(insn, regs); 421 break; 422 case ISA_MULLO_A1: /* mullo Rsrc1, Rsrc2 */ 423 res = emu_mullo_a1(insn, regs); 424 break; 425 default: 426 break; 427 } 428 429 if (!res) 430 return 0; 431 432 switch(insn & 0x70ff) { 433 case ISA_MVFACMI_A0: /* mvfacmi Rdest */ 434 res = emu_mvfacmi_a0(insn, regs); 435 break; 436 case ISA_MVFACMI_A1: /* mvfacmi Rdest */ 437 res = emu_mvfacmi_a1(insn, regs); 438 break; 439 default: 440 break; 441 } 442 443 return res; 444} 445 446#endif /* CONFIG_ISA_DUAL_ISSUE */ 447 448/* 449 * ld : ?010 dest 1100 src 450 * 0010 dest 1110 src : ld Rdest, @Rsrc+ 451 * ldh : ?010 dest 1010 src 452 * lduh : ?010 dest 1011 src 453 * st : ?010 src1 0100 src2 454 * 0010 src1 0110 src2 : st Rsrc1, @+Rsrc2 455 * 0010 src1 0111 src2 : st Rsrc1, @-Rsrc2 456 * sth : ?010 src1 0010 src2 457 */ 458 459static int insn_check(unsigned long insn, struct pt_regs *regs, 460 unsigned char **ucp) 461{ 462 int res = 0; 463 464 /* 465 * 32bit insn 466 * ld Rdest, @(disp16, Rsrc) 467 * st Rdest, @(disp16, Rsrc) 468 */ 469 if (insn & 0x80000000) { /* 32bit insn */ 470 *ucp += (short)(insn & 0x0000ffff); 471 regs->bpc += 4; 472 } else { /* 16bit insn */ 473#ifdef CONFIG_ISA_DUAL_ISSUE 474 /* parallel exec check */ 475 if (!(regs->bpc & 0x2) && insn & 0x8000) { 476 res = emu_m32r2((unsigned short)insn, regs); 477 regs->bpc += 4; 478 } else 479#endif /* CONFIG_ISA_DUAL_ISSUE */ 480 regs->bpc += 2; 481 } 482 483 return res; 484} 485 486static int emu_ld(unsigned long insn32, struct pt_regs *regs) 487{ 488 unsigned char *ucp; 489 unsigned long val; 490 unsigned short insn16; 491 int size, src; 492 493 insn16 = insn32 >> 16; 494 src = REG2(insn16); 495 ucp = (unsigned char *)get_reg(regs, src); 496 497 if (insn_check(insn32, regs, &ucp)) 498 return -1; 499 500 size = insn16 & 0x0040 ? 4 : 2; 501 if (copy_from_user(&val, ucp, size)) 502 return -1; 503 504 if (size == 2) 505 val >>= 16; 506 507 /* ldh sign check */ 508 if ((insn16 & 0x00f0) == 0x00a0 && (val & 0x8000)) 509 val |= 0xffff0000; 510 511 set_reg(regs, REG1(insn16), val); 512 513 /* ld increment check */ 514 if ((insn16 & 0xf0f0) == ISA_LD2) /* ld Rdest, @Rsrc+ */ 515 set_reg(regs, src, (unsigned long)(ucp + 4)); 516 517 return 0; 518} 519 520static int emu_st(unsigned long insn32, struct pt_regs *regs) 521{ 522 unsigned char *ucp; 523 unsigned long val; 524 unsigned short insn16; 525 int size, src2; 526 527 insn16 = insn32 >> 16; 528 src2 = REG2(insn16); 529 530 ucp = (unsigned char *)get_reg(regs, src2); 531 532 if (insn_check(insn32, regs, &ucp)) 533 return -1; 534 535 size = insn16 & 0x0040 ? 4 : 2; 536 val = get_reg(regs, REG1(insn16)); 537 if (size == 2) 538 val <<= 16; 539 540 /* st inc/dec check */ 541 if ((insn16 & 0xf0e0) == 0x2060) { 542 if (insn16 & 0x0010) 543 ucp -= 4; 544 else 545 ucp += 4; 546 547 set_reg(regs, src2, (unsigned long)ucp); 548 } 549 550 if (copy_to_user(ucp, &val, size)) 551 return -1; 552 553 /* sth inc check */ 554 if ((insn16 & 0xf0f0) == ISA_STH2) { 555 ucp += 2; 556 set_reg(regs, src2, (unsigned long)ucp); 557 } 558 559 return 0; 560} 561 562int handle_unaligned_access(unsigned long insn32, struct pt_regs *regs) 563{ 564 unsigned short insn16; 565 int res; 566 567 insn16 = insn32 >> 16; 568 569 /* ld or st check */ 570 if ((insn16 & 0x7000) != 0x2000) 571 return -1; 572 573 /* insn alignment check */ 574 if ((insn16 & 0x8000) && (regs->bpc & 3)) 575 return -1; 576 577 if (insn16 & 0x0080) /* ld */ 578 res = emu_ld(insn32, regs); 579 else /* st */ 580 res = emu_st(insn32, regs); 581 582 return res; 583} 584