LXR linux/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
  10static 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
  26static 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
  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
 585