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 <asm/ptrace.h>
   8 #include <asm/uaccess.h>
   9
  10 static int get_reg(struct pt_regs *regs, int nr)
  11 {
  12         int val;
  13
  14         if (nr < 4)
  15                 val = *(unsigned long *)(&regs->r0 + nr);
  16         else if (nr < 7)
  17                 val = *(unsigned long *)(&regs->r4 + (nr - 4));
  18         else if (nr < 13)
  19                 val = *(unsigned long *)(&regs->r7 + (nr - 7));
  20         else
  21                 val = *(unsigned long *)(&regs->fp + (nr - 13));
  22
  23         return val;
  24 }
  25
  26 static void set_reg(struct pt_regs *regs, int nr, int val)
  27 {
  28         if (nr < 4)
  29                 *(unsigned long *)(&regs->r0 + nr) = val;
  30         else if (nr < 7)
  31                 *(unsigned long *)(&regs->r4 + (nr - 4)) = val;
  32         else if (nr < 13)
  33                 *(unsigned long *)(&regs->r7 + (nr - 7)) = val;
  34         else
  35                 *(unsigned long *)(&regs->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
  81 static 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
  94 static 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
 103 static 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
 116 static 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
 136 static 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
 148 static 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
 158 static 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
 168 static 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
 179 static 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
 189 static 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
 199 static 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
 209 static 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
 219 static 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
 231 static 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
 243 static 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
 262 static 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
 274 static 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
 292 static 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
 312 static 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
 332 static 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
 342 static 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
 352 static 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
 459 static 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
 486 static 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
 520 static 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
 562 int 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