arch/sh/kernel/traps_32.c

   1 /*
   2  * 'traps.c' handles hardware traps and faults after we have saved some
   3  * state in 'entry.S'.
   4  *
   5  *  SuperH version: Copyright (C) 1999 Niibe Yutaka
   6  *                  Copyright (C) 2000 Philipp Rumpf
   7  *                  Copyright (C) 2000 David Howells
   8  *                  Copyright (C) 2002 - 2010 Paul Mundt
   9  *
  10  * This file is subject to the terms and conditions of the GNU General Public
  11  * License.  See the file "COPYING" in the main directory of this archive
  12  * for more details.
  13  */
  14 #include <linux/kernel.h>
  15 #include <linux/ptrace.h>
  16 #include <linux/hardirq.h>
  17 #include <linux/init.h>
  18 #include <linux/spinlock.h>
  19 #include <linux/kallsyms.h>
  20 #include <linux/io.h>
  21 #include <linux/bug.h>
  22 #include <linux/debug_locks.h>
  23 #include <linux/kdebug.h>
  24 #include <linux/limits.h>
  25 #include <linux/sysfs.h>
  26 #include <linux/uaccess.h>
  27 #include <linux/perf_event.h>
  28 #include <asm/alignment.h>
  29 #include <asm/fpu.h>
  30 #include <asm/kprobes.h>
  31 #include <asm/traps.h>
  32 #include <asm/bl_bit.h>
  33
  34 #ifdef CONFIG_CPU_SH2
  35 # define TRAP_RESERVED_INST     4
  36 # define TRAP_ILLEGAL_SLOT_INST 6
  37 # define TRAP_ADDRESS_ERROR     9
  38 # ifdef CONFIG_CPU_SH2A
  39 #  define TRAP_UBC              12
  40 #  define TRAP_FPU_ERROR        13
  41 #  define TRAP_DIVZERO_ERROR    17
  42 #  define TRAP_DIVOVF_ERROR     18
  43 # endif
  44 #else
  45 #define TRAP_RESERVED_INST      12
  46 #define TRAP_ILLEGAL_SLOT_INST  13
  47 #endif
  48
  49 static inline void sign_extend(unsigned int count, unsigned char *dst)
  50 {
  51 #ifdef __LITTLE_ENDIAN__
  52         if ((count == 1) && dst[0] & 0x80) {
  53                 dst[1] = 0xff;
  54                 dst[2] = 0xff;
  55                 dst[3] = 0xff;
  56         }
  57         if ((count == 2) && dst[1] & 0x80) {
  58                 dst[2] = 0xff;
  59                 dst[3] = 0xff;
  60         }
  61 #else
  62         if ((count == 1) && dst[3] & 0x80) {
  63                 dst[2] = 0xff;
  64                 dst[1] = 0xff;
  65                 dst[0] = 0xff;
  66         }
  67         if ((count == 2) && dst[2] & 0x80) {
  68                 dst[1] = 0xff;
  69                 dst[0] = 0xff;
  70         }
  71 #endif
  72 }
  73
  74 static struct mem_access user_mem_access = {
  75         copy_from_user,
  76         copy_to_user,
  77 };
  78
  79 /*
  80  * handle an instruction that does an unaligned memory access by emulating the
  81  * desired behaviour
  82  * - note that PC _may not_ point to the faulting instruction
  83  *   (if that instruction is in a branch delay slot)
  84  * - return 0 if emulation okay, -EFAULT on existential error
  85  */
  86 static int handle_unaligned_ins(insn_size_t instruction, struct pt_regs *regs,
  87                                 struct mem_access *ma)
  88 {
  89         int ret, index, count;
  90         unsigned long *rm, *rn;
  91         unsigned char *src, *dst;
  92         unsigned char __user *srcu, *dstu;
  93
  94         index = (instruction>>8)&15;    /* 0x0F00 */
  95         rn = &regs->regs[index];
  96
  97         index = (instruction>>4)&15;    /* 0x00F0 */
  98         rm = &regs->regs[index];
  99
 100         count = 1<<(instruction&3);
 101
 102         switch (count) {
 103         case 1: inc_unaligned_byte_access(); break;
 104         case 2: inc_unaligned_word_access(); break;
 105         case 4: inc_unaligned_dword_access(); break;
 106         case 8: inc_unaligned_multi_access(); break;
 107         }
 108
 109         ret = -EFAULT;
 110         switch (instruction>>12) {
 111         case 0: /* mov.[bwl] to/from memory via r0+rn */
 112                 if (instruction & 8) {
 113                         /* from memory */
 114                         srcu = (unsigned char __user *)*rm;
 115                         srcu += regs->regs[0];
 116                         dst = (unsigned char *)rn;
 117                         *(unsigned long *)dst = 0;
 118
 119 #if !defined(__LITTLE_ENDIAN__)
 120                         dst += 4-count;
 121 #endif
 122                         if (ma->from(dst, srcu, count))
 123                                 goto fetch_fault;
 124
 125                         sign_extend(count, dst);
 126                 } else {
 127                         /* to memory */
 128                         src = (unsigned char *)rm;
 129 #if !defined(__LITTLE_ENDIAN__)
 130                         src += 4-count;
 131 #endif
 132                         dstu = (unsigned char __user *)*rn;
 133                         dstu += regs->regs[0];
 134
 135                         if (ma->to(dstu, src, count))
 136                                 goto fetch_fault;
 137                 }
 138                 ret = 0;
 139                 break;
 140
 141         case 1: /* mov.l Rm,@(disp,Rn) */
 142                 src = (unsigned char*) rm;
 143                 dstu = (unsigned char __user *)*rn;
 144                 dstu += (instruction&0x000F)<<2;
 145
 146                 if (ma->to(dstu, src, 4))
 147                         goto fetch_fault;
 148                 ret = 0;
 149                 break;
 150
 151         case 2: /* mov.[bwl] to memory, possibly with pre-decrement */
 152                 if (instruction & 4)
 153                         *rn -= count;
 154                 src = (unsigned char*) rm;
 155                 dstu = (unsigned char __user *)*rn;
 156 #if !defined(__LITTLE_ENDIAN__)
 157                 src += 4-count;
 158 #endif
 159                 if (ma->to(dstu, src, count))
 160                         goto fetch_fault;
 161                 ret = 0;
 162                 break;
 163
 164         case 5: /* mov.l @(disp,Rm),Rn */
 165                 srcu = (unsigned char __user *)*rm;
 166                 srcu += (instruction & 0x000F) << 2;
 167                 dst = (unsigned char *)rn;
 168                 *(unsigned long *)dst = 0;
 169
 170                 if (ma->from(dst, srcu, 4))
 171                         goto fetch_fault;
 172                 ret = 0;
 173                 break;
 174
 175         case 6: /* mov.[bwl] from memory, possibly with post-increment */
 176                 srcu = (unsigned char __user *)*rm;
 177                 if (instruction & 4)
 178                         *rm += count;
 179                 dst = (unsigned char*) rn;
 180                 *(unsigned long*)dst = 0;
 181
 182 #if !defined(__LITTLE_ENDIAN__)
 183                 dst += 4-count;
 184 #endif
 185                 if (ma->from(dst, srcu, count))
 186                         goto fetch_fault;
 187                 sign_extend(count, dst);
 188                 ret = 0;
 189                 break;
 190
 191         case 8:
 192                 switch ((instruction&0xFF00)>>8) {
 193                 case 0x81: /* mov.w R0,@(disp,Rn) */
 194                         src = (unsigned char *) &regs->regs[0];
 195 #if !defined(__LITTLE_ENDIAN__)
 196                         src += 2;
 197 #endif
 198                         dstu = (unsigned char __user *)*rm; /* called Rn in the spec */
 199                         dstu += (instruction & 0x000F) << 1;
 200
 201                         if (ma->to(dstu, src, 2))
 202                                 goto fetch_fault;
 203                         ret = 0;
 204                         break;
 205
 206                 case 0x85: /* mov.w @(disp,Rm),R0 */
 207                         srcu = (unsigned char __user *)*rm;
 208                         srcu += (instruction & 0x000F) << 1;
 209                         dst = (unsigned char *) &regs->regs[0];
 210                         *(unsigned long *)dst = 0;
 211
 212 #if !defined(__LITTLE_ENDIAN__)
 213                         dst += 2;
 214 #endif
 215                         if (ma->from(dst, srcu, 2))
 216                                 goto fetch_fault;
 217                         sign_extend(2, dst);
 218                         ret = 0;
 219                         break;
 220                 }
 221                 break;
 222
 223         case 9: /* mov.w @(disp,PC),Rn */
 224                 srcu = (unsigned char __user *)regs->pc;
 225                 srcu += 4;
 226                 srcu += (instruction & 0x00FF) << 1;
 227                 dst = (unsigned char *)rn;
 228                 *(unsigned long *)dst = 0;
 229
 230 #if !defined(__LITTLE_ENDIAN__)
 231                 dst += 2;
 232 #endif
 233
 234                 if (ma->from(dst, srcu, 2))
 235                         goto fetch_fault;
 236                 sign_extend(2, dst);
 237                 ret = 0;
 238                 break;
 239
 240         case 0xd: /* mov.l @(disp,PC),Rn */
 241                 srcu = (unsigned char __user *)(regs->pc & ~0x3);
 242                 srcu += 4;
 243                 srcu += (instruction & 0x00FF) << 2;
 244                 dst = (unsigned char *)rn;
 245                 *(unsigned long *)dst = 0;
 246
 247                 if (ma->from(dst, srcu, 4))
 248                         goto fetch_fault;
 249                 ret = 0;
 250                 break;
 251         }
 252         return ret;
 253
 254  fetch_fault:
 255         /* Argh. Address not only misaligned but also non-existent.
 256          * Raise an EFAULT and see if it's trapped
 257          */
 258         die_if_no_fixup("Fault in unaligned fixup", regs, 0);
 259         return -EFAULT;
 260 }
 261
 262 /*
 263  * emulate the instruction in the delay slot
 264  * - fetches the instruction from PC+2
 265  */
 266 static inline int handle_delayslot(struct pt_regs *regs,
 267                                    insn_size_t old_instruction,
 268                                    struct mem_access *ma)
 269 {
 270         insn_size_t instruction;
 271         void __user *addr = (void __user *)(regs->pc +
 272                 instruction_size(old_instruction));
 273
 274         if (copy_from_user(&instruction, addr, sizeof(instruction))) {
 275                 /* the instruction-fetch faulted */
 276                 if (user_mode(regs))
 277                         return -EFAULT;
 278
 279                 /* kernel */
 280                 die("delay-slot-insn faulting in handle_unaligned_delayslot",
 281                     regs, 0);
 282         }
 283
 284         return handle_unaligned_ins(instruction, regs, ma);
 285 }
 286
 287 /*
 288  * handle an instruction that does an unaligned memory access
 289  * - have to be careful of branch delay-slot instructions that fault
 290  *  SH3:
 291  *   - if the branch would be taken PC points to the branch
 292  *   - if the branch would not be taken, PC points to delay-slot
 293  *  SH4:
 294  *   - PC always points to delayed branch
 295  * - return 0 if handled, -EFAULT if failed (may not return if in kernel)
 296  */
 297
 298 /* Macros to determine offset from current PC for branch instructions */
 299 /* Explicit type coercion is used to force sign extension where needed */
 300 #define SH_PC_8BIT_OFFSET(instr) ((((signed char)(instr))*2) + 4)
 301 #define SH_PC_12BIT_OFFSET(instr) ((((signed short)(instr<<4))>>3) + 4)
 302
 303 int handle_unaligned_access(insn_size_t instruction, struct pt_regs *regs,
 304                             struct mem_access *ma, int expected,
 305                             unsigned long address)
 306 {
 307         u_int rm;
 308         int ret, index;
 309
 310         /*
 311          * XXX: We can't handle mixed 16/32-bit instructions yet
 312          */
 313         if (instruction_size(instruction) != 2)
 314                 return -EINVAL;
 315
 316         index = (instruction>>8)&15;    /* 0x0F00 */
 317         rm = regs->regs[index];
 318
 319         /*
 320          * Log the unexpected fixups, and then pass them on to perf.
 321          *
 322          * We intentionally don't report the expected cases to perf as
 323          * otherwise the trapped I/O case will skew the results too much
 324          * to be useful.
 325          */
 326         if (!expected) {
 327                 unaligned_fixups_notify(current, instruction, regs);
 328                 perf_sw_event(PERF_COUNT_SW_ALIGNMENT_FAULTS, 1,
 329                               regs, address);
 330         }
 331
 332         ret = -EFAULT;
 333         switch (instruction&0xF000) {
 334         case 0x0000:
 335                 if (instruction==0x000B) {
 336                         /* rts */
 337                         ret = handle_delayslot(regs, instruction, ma);
 338                         if (ret==0)
 339                                 regs->pc = regs->pr;
 340                 }
 341                 else if ((instruction&0x00FF)==0x0023) {
 342                         /* braf @Rm */
 343                         ret = handle_delayslot(regs, instruction, ma);
 344                         if (ret==0)
 345                                 regs->pc += rm + 4;
 346                 }
 347                 else if ((instruction&0x00FF)==0x0003) {
 348                         /* bsrf @Rm */
 349                         ret = handle_delayslot(regs, instruction, ma);
 350                         if (ret==0) {
 351                                 regs->pr = regs->pc + 4;
 352                                 regs->pc += rm + 4;
 353                         }
 354                 }
 355                 else {
 356                         /* mov.[bwl] to/from memory via r0+rn */
 357                         goto simple;
 358                 }
 359                 break;
 360
 361         case 0x1000: /* mov.l Rm,@(disp,Rn) */
 362                 goto simple;
 363
 364         case 0x2000: /* mov.[bwl] to memory, possibly with pre-decrement */
 365                 goto simple;
 366
 367         case 0x4000:
 368                 if ((instruction&0x00FF)==0x002B) {
 369                         /* jmp @Rm */
 370                         ret = handle_delayslot(regs, instruction, ma);
 371                         if (ret==0)
 372                                 regs->pc = rm;
 373                 }
 374                 else if ((instruction&0x00FF)==0x000B) {
 375                         /* jsr @Rm */
 376                         ret = handle_delayslot(regs, instruction, ma);
 377                         if (ret==0) {
 378                                 regs->pr = regs->pc + 4;
 379                                 regs->pc = rm;
 380                         }
 381                 }
 382                 else {
 383                         /* mov.[bwl] to/from memory via r0+rn */
 384                         goto simple;
 385                 }
 386                 break;
 387
 388         case 0x5000: /* mov.l @(disp,Rm),Rn */
 389                 goto simple;
 390
 391         case 0x6000: /* mov.[bwl] from memory, possibly with post-increment */
 392                 goto simple;
 393
 394         case 0x8000: /* bf lab, bf/s lab, bt lab, bt/s lab */
 395                 switch (instruction&0x0F00) {
 396                 case 0x0100: /* mov.w R0,@(disp,Rm) */
 397                         goto simple;
 398                 case 0x0500: /* mov.w @(disp,Rm),R0 */
 399                         goto simple;
 400                 case 0x0B00: /* bf   lab - no delayslot*/
 401                         ret = 0;
 402                         break;
 403                 case 0x0F00: /* bf/s lab */
 404                         ret = handle_delayslot(regs, instruction, ma);
 405                         if (ret==0) {
 406 #if defined(CONFIG_CPU_SH4) || defined(CONFIG_SH7705_CACHE_32KB)
 407                                 if ((regs->sr & 0x00000001) != 0)
 408                                         regs->pc += 4; /* next after slot */
 409                                 else
 410 #endif
 411                                         regs->pc += SH_PC_8BIT_OFFSET(instruction);
 412                         }
 413                         break;
 414                 case 0x0900: /* bt   lab - no delayslot */
 415                         ret = 0;
 416                         break;
 417                 case 0x0D00: /* bt/s lab */
 418                         ret = handle_delayslot(regs, instruction, ma);
 419                         if (ret==0) {
 420 #if defined(CONFIG_CPU_SH4) || defined(CONFIG_SH7705_CACHE_32KB)
 421                                 if ((regs->sr & 0x00000001) == 0)
 422                                         regs->pc += 4; /* next after slot */
 423                                 else
 424 #endif
 425                                         regs->pc += SH_PC_8BIT_OFFSET(instruction);
 426                         }
 427                         break;
 428                 }
 429                 break;
 430
 431         case 0x9000: /* mov.w @(disp,Rm),Rn */
 432                 goto simple;
 433
 434         case 0xA000: /* bra label */
 435                 ret = handle_delayslot(regs, instruction, ma);
 436                 if (ret==0)
 437                         regs->pc += SH_PC_12BIT_OFFSET(instruction);
 438                 break;
 439
 440         case 0xB000: /* bsr label */
 441                 ret = handle_delayslot(regs, instruction, ma);
 442                 if (ret==0) {
 443                         regs->pr = regs->pc + 4;
 444                         regs->pc += SH_PC_12BIT_OFFSET(instruction);
 445                 }
 446                 break;
 447
 448         case 0xD000: /* mov.l @(disp,Rm),Rn */
 449                 goto simple;
 450         }
 451         return ret;
 452
 453         /* handle non-delay-slot instruction */
 454  simple:
 455         ret = handle_unaligned_ins(instruction, regs, ma);
 456         if (ret==0)
 457                 regs->pc += instruction_size(instruction);
 458         return ret;
 459 }
 460
 461 /*
 462  * Handle various address error exceptions:
 463  *  - instruction address error:
 464  *       misaligned PC
 465  *       PC >= 0x80000000 in user mode
 466  *  - data address error (read and write)
 467  *       misaligned data access
 468  *       access to >= 0x80000000 is user mode
 469  * Unfortuntaly we can't distinguish between instruction address error
 470  * and data address errors caused by read accesses.
 471  */
 472 asmlinkage void do_address_error(struct pt_regs *regs,
 473                                  unsigned long writeaccess,
 474                                  unsigned long address)
 475 {
 476         unsigned long error_code = 0;
 477         mm_segment_t oldfs;
 478         siginfo_t info;
 479         insn_size_t instruction;
 480         int tmp;
 481
 482         /* Intentional ifdef */
 483 #ifdef CONFIG_CPU_HAS_SR_RB
 484         error_code = lookup_exception_vector();
 485 #endif
 486
 487         oldfs = get_fs();
 488
 489         if (user_mode(regs)) {
 490                 int si_code = BUS_ADRERR;
 491                 unsigned int user_action;
 492
 493                 local_irq_enable();
 494                 inc_unaligned_user_access();
 495
 496                 set_fs(USER_DS);
 497                 if (copy_from_user(&instruction, (insn_size_t *)(regs->pc & ~1),
 498                                    sizeof(instruction))) {
 499                         set_fs(oldfs);
 500                         goto uspace_segv;
 501                 }
 502                 set_fs(oldfs);
 503
 504                 /* shout about userspace fixups */
 505                 unaligned_fixups_notify(current, instruction, regs);
 506
 507                 user_action = unaligned_user_action();
 508                 if (user_action & UM_FIXUP)
 509                         goto fixup;
 510                 if (user_action & UM_SIGNAL)
 511                         goto uspace_segv;
 512                 else {
 513                         /* ignore */
 514                         regs->pc += instruction_size(instruction);
 515                         return;
 516                 }
 517
 518 fixup:
 519                 /* bad PC is not something we can fix */
 520                 if (regs->pc & 1) {
 521                         si_code = BUS_ADRALN;
 522                         goto uspace_segv;
 523                 }
 524
 525                 set_fs(USER_DS);
 526                 tmp = handle_unaligned_access(instruction, regs,
 527                                               &user_mem_access, 0,
 528                                               address);
 529                 set_fs(oldfs);
 530
 531                 if (tmp == 0)
 532                         return; /* sorted */
 533 uspace_segv:
 534                 printk(KERN_NOTICE "Sending SIGBUS to \"%s\" due to unaligned "
 535                        "access (PC %lx PR %lx)\n", current->comm, regs->pc,
 536                        regs->pr);
 537
 538                 info.si_signo = SIGBUS;
 539                 info.si_errno = 0;
 540                 info.si_code = si_code;
 541                 info.si_addr = (void __user *)address;
 542                 force_sig_info(SIGBUS, &info, current);
 543         } else {
 544                 inc_unaligned_kernel_access();
 545
 546                 if (regs->pc & 1)
 547                         die("unaligned program counter", regs, error_code);
 548
 549                 set_fs(KERNEL_DS);
 550                 if (copy_from_user(&instruction, (void __user *)(regs->pc),
 551                                    sizeof(instruction))) {
 552                         /* Argh. Fault on the instruction itself.
 553                            This should never happen non-SMP
 554                         */
 555                         set_fs(oldfs);
 556                         die("insn faulting in do_address_error", regs, 0);
 557                 }
 558
 559                 unaligned_fixups_notify(current, instruction, regs);
 560
 561                 handle_unaligned_access(instruction, regs, &user_mem_access,
 562                                         0, address);
 563                 set_fs(oldfs);
 564         }
 565 }
 566
 567 #ifdef CONFIG_SH_DSP
 568 /*
 569  *      SH-DSP support gerg@snapgear.com.
 570  */
 571 int is_dsp_inst(struct pt_regs *regs)
 572 {
 573         unsigned short inst = 0;
 574
 575         /*
 576          * Safe guard if DSP mode is already enabled or we're lacking
 577          * the DSP altogether.
 578          */
 579         if (!(current_cpu_data.flags & CPU_HAS_DSP) || (regs->sr & SR_DSP))
 580                 return 0;
 581
 582         get_user(inst, ((unsigned short *) regs->pc));
 583
 584         inst &= 0xf000;
 585
 586         /* Check for any type of DSP or support instruction */
 587         if ((inst == 0xf000) || (inst == 0x4000))
 588                 return 1;
 589
 590         return 0;
 591 }
 592 #else
 593 #define is_dsp_inst(regs)       (0)
 594 #endif /* CONFIG_SH_DSP */
 595
 596 #ifdef CONFIG_CPU_SH2A
 597 asmlinkage void do_divide_error(unsigned long r4, unsigned long r5,
 598                                 unsigned long r6, unsigned long r7,
 599                                 struct pt_regs __regs)
 600 {
 601         siginfo_t info;
 602
 603         switch (r4) {
 604         case TRAP_DIVZERO_ERROR:
 605                 info.si_code = FPE_INTDIV;
 606                 break;
 607         case TRAP_DIVOVF_ERROR:
 608                 info.si_code = FPE_INTOVF;
 609                 break;
 610         }
 611
 612         force_sig_info(SIGFPE, &info, current);
 613 }
 614 #endif
 615
 616 asmlinkage void do_reserved_inst(unsigned long r4, unsigned long r5,
 617                                 unsigned long r6, unsigned long r7,
 618                                 struct pt_regs __regs)
 619 {
 620         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 621         unsigned long error_code;
 622         struct task_struct *tsk = current;
 623
 624 #ifdef CONFIG_SH_FPU_EMU
 625         unsigned short inst = 0;
 626         int err;
 627
 628         get_user(inst, (unsigned short*)regs->pc);
 629
 630         err = do_fpu_inst(inst, regs);
 631         if (!err) {
 632                 regs->pc += instruction_size(inst);
 633                 return;
 634         }
 635         /* not a FPU inst. */
 636 #endif
 637
 638 #ifdef CONFIG_SH_DSP
 639         /* Check if it's a DSP instruction */
 640         if (is_dsp_inst(regs)) {
 641                 /* Enable DSP mode, and restart instruction. */
 642                 regs->sr |= SR_DSP;
 643                 /* Save DSP mode */
 644                 tsk->thread.dsp_status.status |= SR_DSP;
 645                 return;
 646         }
 647 #endif
 648
 649         error_code = lookup_exception_vector();
 650
 651         local_irq_enable();
 652         force_sig(SIGILL, tsk);
 653         die_if_no_fixup("reserved instruction", regs, error_code);
 654 }
 655
 656 #ifdef CONFIG_SH_FPU_EMU
 657 static int emulate_branch(unsigned short inst, struct pt_regs *regs)
 658 {
 659         /*
 660          * bfs: 8fxx: PC+=d*2+4;
 661          * bts: 8dxx: PC+=d*2+4;
 662          * bra: axxx: PC+=D*2+4;
 663          * bsr: bxxx: PC+=D*2+4  after PR=PC+4;
 664          * braf:0x23: PC+=Rn*2+4;
 665          * bsrf:0x03: PC+=Rn*2+4 after PR=PC+4;
 666          * jmp: 4x2b: PC=Rn;
 667          * jsr: 4x0b: PC=Rn      after PR=PC+4;
 668          * rts: 000b: PC=PR;
 669          */
 670         if (((inst & 0xf000) == 0xb000)  ||     /* bsr */
 671             ((inst & 0xf0ff) == 0x0003)  ||     /* bsrf */
 672             ((inst & 0xf0ff) == 0x400b))        /* jsr */
 673                 regs->pr = regs->pc + 4;
 674
 675         if ((inst & 0xfd00) == 0x8d00) {        /* bfs, bts */
 676                 regs->pc += SH_PC_8BIT_OFFSET(inst);
 677                 return 0;
 678         }
 679
 680         if ((inst & 0xe000) == 0xa000) {        /* bra, bsr */
 681                 regs->pc += SH_PC_12BIT_OFFSET(inst);
 682                 return 0;
 683         }
 684
 685         if ((inst & 0xf0df) == 0x0003) {        /* braf, bsrf */
 686                 regs->pc += regs->regs[(inst & 0x0f00) >> 8] + 4;
 687                 return 0;
 688         }
 689
 690         if ((inst & 0xf0df) == 0x400b) {        /* jmp, jsr */
 691                 regs->pc = regs->regs[(inst & 0x0f00) >> 8];
 692                 return 0;
 693         }
 694
 695         if ((inst & 0xffff) == 0x000b) {        /* rts */
 696                 regs->pc = regs->pr;
 697                 return 0;
 698         }
 699
 700         return 1;
 701 }
 702 #endif
 703
 704 asmlinkage void do_illegal_slot_inst(unsigned long r4, unsigned long r5,
 705                                 unsigned long r6, unsigned long r7,
 706                                 struct pt_regs __regs)
 707 {
 708         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 709         unsigned long inst;
 710         struct task_struct *tsk = current;
 711
 712         if (kprobe_handle_illslot(regs->pc) == 0)
 713                 return;
 714
 715 #ifdef CONFIG_SH_FPU_EMU
 716         get_user(inst, (unsigned short *)regs->pc + 1);
 717         if (!do_fpu_inst(inst, regs)) {
 718                 get_user(inst, (unsigned short *)regs->pc);
 719                 if (!emulate_branch(inst, regs))
 720                         return;
 721                 /* fault in branch.*/
 722         }
 723         /* not a FPU inst. */
 724 #endif
 725
 726         inst = lookup_exception_vector();
 727
 728         local_irq_enable();
 729         force_sig(SIGILL, tsk);
 730         die_if_no_fixup("illegal slot instruction", regs, inst);
 731 }
 732
 733 asmlinkage void do_exception_error(unsigned long r4, unsigned long r5,
 734                                    unsigned long r6, unsigned long r7,
 735                                    struct pt_regs __regs)
 736 {
 737         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
 738         long ex;
 739
 740         ex = lookup_exception_vector();
 741         die_if_kernel("exception", regs, ex);
 742 }
 743
 744 void __cpuinit per_cpu_trap_init(void)
 745 {
 746         extern void *vbr_base;
 747
 748         /* NOTE: The VBR value should be at P1
 749            (or P2, virtural "fixed" address space).
 750            It's definitely should not in physical address.  */
 751
 752         asm volatile("ldc       %0, vbr"
 753                      : /* no output */
 754                      : "r" (&vbr_base)
 755                      : "memory");
 756
 757         /* disable exception blocking now when the vbr has been setup */
 758         clear_bl_bit();
 759 }
 760
 761 void *set_exception_table_vec(unsigned int vec, void *handler)
 762 {
 763         extern void *exception_handling_table[];
 764         void *old_handler;
 765
 766         old_handler = exception_handling_table[vec];
 767         exception_handling_table[vec] = handler;
 768         return old_handler;
 769 }
 770
 771 void __init trap_init(void)
 772 {
 773         set_exception_table_vec(TRAP_RESERVED_INST, do_reserved_inst);
 774         set_exception_table_vec(TRAP_ILLEGAL_SLOT_INST, do_illegal_slot_inst);
 775
 776 #if defined(CONFIG_CPU_SH4) && !defined(CONFIG_SH_FPU) || \
 777     defined(CONFIG_SH_FPU_EMU)
 778         /*
 779          * For SH-4 lacking an FPU, treat floating point instructions as
 780          * reserved. They'll be handled in the math-emu case, or faulted on
 781          * otherwise.
 782          */
 783         set_exception_table_evt(0x800, do_reserved_inst);
 784         set_exception_table_evt(0x820, do_illegal_slot_inst);
 785 #elif defined(CONFIG_SH_FPU)
 786         set_exception_table_evt(0x800, fpu_state_restore_trap_handler);
 787         set_exception_table_evt(0x820, fpu_state_restore_trap_handler);
 788 #endif
 789
 790 #ifdef CONFIG_CPU_SH2
 791         set_exception_table_vec(TRAP_ADDRESS_ERROR, address_error_trap_handler);
 792 #endif
 793 #ifdef CONFIG_CPU_SH2A
 794         set_exception_table_vec(TRAP_DIVZERO_ERROR, do_divide_error);
 795         set_exception_table_vec(TRAP_DIVOVF_ERROR, do_divide_error);
 796 #ifdef CONFIG_SH_FPU
 797         set_exception_table_vec(TRAP_FPU_ERROR, fpu_error_trap_handler);
 798 #endif
 799 #endif
 800
 801 #ifdef TRAP_UBC
 802         set_exception_table_vec(TRAP_UBC, breakpoint_trap_handler);
 803 #endif
 804 }