arch/m32r/kernel/align.c

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