arch/arm/mm/alignment.c

   1 /*
   2  *  linux/arch/arm/mm/alignment.c
   3  *
   4  *  Copyright (C) 1995  Linus Torvalds
   5  *  Modifications for ARM processor (c) 1995-2001 Russell King
   6  *  Thumb aligment fault fixups (c) 2004 MontaVista Software, Inc.
   7  *  - Adapted from gdb/sim/arm/thumbemu.c -- Thumb instruction emulation.
   8  *    Copyright (C) 1996, Cygnus Software Technologies Ltd.
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License version 2 as
  12  * published by the Free Software Foundation.
  13  */
  14 #include <linux/compiler.h>
  15 #include <linux/kernel.h>
  16 #include <linux/errno.h>
  17 #include <linux/string.h>
  18 #include <linux/ptrace.h>
  19 #include <linux/proc_fs.h>
  20 #include <linux/init.h>
  21
  22 #include <asm/uaccess.h>
  23 #include <asm/unaligned.h>
  24
  25 #include "fault.h"
  26
  27 /*
  28  * 32-bit misaligned trap handler (c) 1998 San Mehat (CCC) -July 1998
  29  * /proc/sys/debug/alignment, modified and integrated into
  30  * Linux 2.1 by Russell King
  31  *
  32  * Speed optimisations and better fault handling by Russell King.
  33  *
  34  * *** NOTE ***
  35  * This code is not portable to processors with late data abort handling.
  36  */
  37 #define CODING_BITS(i)  (i & 0x0e000000)
  38
  39 #define LDST_I_BIT(i)   (i & (1 << 26))         /* Immediate constant   */
  40 #define LDST_P_BIT(i)   (i & (1 << 24))         /* Preindex             */
  41 #define LDST_U_BIT(i)   (i & (1 << 23))         /* Add offset           */
  42 #define LDST_W_BIT(i)   (i & (1 << 21))         /* Writeback            */
  43 #define LDST_L_BIT(i)   (i & (1 << 20))         /* Load                 */
  44
  45 #define LDST_P_EQ_U(i)  ((((i) ^ ((i) >> 1)) & (1 << 23)) == 0)
  46
  47 #define LDSTHD_I_BIT(i) (i & (1 << 22))         /* double/half-word immed */
  48 #define LDM_S_BIT(i)    (i & (1 << 22))         /* write CPSR from SPSR */
  49
  50 #define RN_BITS(i)      ((i >> 16) & 15)        /* Rn                   */
  51 #define RD_BITS(i)      ((i >> 12) & 15)        /* Rd                   */
  52 #define RM_BITS(i)      (i & 15)                /* Rm                   */
  53
  54 #define REGMASK_BITS(i) (i & 0xffff)
  55 #define OFFSET_BITS(i)  (i & 0x0fff)
  56
  57 #define IS_SHIFT(i)     (i & 0x0ff0)
  58 #define SHIFT_BITS(i)   ((i >> 7) & 0x1f)
  59 #define SHIFT_TYPE(i)   (i & 0x60)
  60 #define SHIFT_LSL       0x00
  61 #define SHIFT_LSR       0x20
  62 #define SHIFT_ASR       0x40
  63 #define SHIFT_RORRRX    0x60
  64
  65 static unsigned long ai_user;
  66 static unsigned long ai_sys;
  67 static unsigned long ai_skipped;
  68 static unsigned long ai_half;
  69 static unsigned long ai_word;
  70 static unsigned long ai_dword;
  71 static unsigned long ai_multi;
  72 static int ai_usermode;
  73
  74 #ifdef CONFIG_PROC_FS
  75 static const char *usermode_action[] = {
  76         "ignored",
  77         "warn",
  78         "fixup",
  79         "fixup+warn",
  80         "signal",
  81         "signal+warn"
  82 };
  83
  84 static int
  85 proc_alignment_read(char *page, char **start, off_t off, int count, int *eof,
  86                     void *data)
  87 {
  88         char *p = page;
  89         int len;
  90
  91         p += sprintf(p, "User:\t\t%lu\n", ai_user);
  92         p += sprintf(p, "System:\t\t%lu\n", ai_sys);
  93         p += sprintf(p, "Skipped:\t%lu\n", ai_skipped);
  94         p += sprintf(p, "Half:\t\t%lu\n", ai_half);
  95         p += sprintf(p, "Word:\t\t%lu\n", ai_word);
  96         if (cpu_architecture() >= CPU_ARCH_ARMv5TE)
  97                 p += sprintf(p, "DWord:\t\t%lu\n", ai_dword);
  98         p += sprintf(p, "Multi:\t\t%lu\n", ai_multi);
  99         p += sprintf(p, "User faults:\t%i (%s)\n", ai_usermode,
 100                         usermode_action[ai_usermode]);
 101
 102         len = (p - page) - off;
 103         if (len < 0)
 104                 len = 0;
 105
 106         *eof = (len <= count) ? 1 : 0;
 107         *start = page + off;
 108
 109         return len;
 110 }
 111
 112 static int proc_alignment_write(struct file *file, const char __user *buffer,
 113                                 unsigned long count, void *data)
 114 {
 115         char mode;
 116
 117         if (count > 0) {
 118                 if (get_user(mode, buffer))
 119                         return -EFAULT;
 120                 if (mode >= '0' && mode <= '5')
 121                         ai_usermode = mode - '0';
 122         }
 123         return count;
 124 }
 125
 126 #endif /* CONFIG_PROC_FS */
 127
 128 union offset_union {
 129         unsigned long un;
 130           signed long sn;
 131 };
 132
 133 #define TYPE_ERROR      0
 134 #define TYPE_FAULT      1
 135 #define TYPE_LDST       2
 136 #define TYPE_DONE       3
 137
 138 #ifdef __ARMEB__
 139 #define BE              1
 140 #define FIRST_BYTE_16   "mov    %1, %1, ror #8\n"
 141 #define FIRST_BYTE_32   "mov    %1, %1, ror #24\n"
 142 #define NEXT_BYTE       "ror #24"
 143 #else
 144 #define BE              0
 145 #define FIRST_BYTE_16
 146 #define FIRST_BYTE_32
 147 #define NEXT_BYTE       "lsr #8"
 148 #endif
 149
 150 #define __get8_unaligned_check(ins,val,addr,err)        \
 151         __asm__(                                        \
 152         "1:     "ins"   %1, [%2], #1\n"                 \
 153         "2:\n"                                          \
 154         "       .section .fixup,\"ax\"\n"               \
 155         "       .align  2\n"                            \
 156         "3:     mov     %0, #1\n"                       \
 157         "       b       2b\n"                           \
 158         "       .previous\n"                            \
 159         "       .section __ex_table,\"a\"\n"            \
 160         "       .align  3\n"                            \
 161         "       .long   1b, 3b\n"                       \
 162         "       .previous\n"                            \
 163         : "=r" (err), "=&r" (val), "=r" (addr)          \
 164         : "0" (err), "2" (addr))
 165
 166 #define __get16_unaligned_check(ins,val,addr)                   \
 167         do {                                                    \
 168                 unsigned int err = 0, v, a = addr;              \
 169                 __get8_unaligned_check(ins,v,a,err);            \
 170                 val =  v << ((BE) ? 8 : 0);                     \
 171                 __get8_unaligned_check(ins,v,a,err);            \
 172                 val |= v << ((BE) ? 0 : 8);                     \
 173                 if (err)                                        \
 174                         goto fault;                             \
 175         } while (0)
 176
 177 #define get16_unaligned_check(val,addr) \
 178         __get16_unaligned_check("ldrb",val,addr)
 179
 180 #define get16t_unaligned_check(val,addr) \
 181         __get16_unaligned_check("ldrbt",val,addr)
 182
 183 #define __get32_unaligned_check(ins,val,addr)                   \
 184         do {                                                    \
 185                 unsigned int err = 0, v, a = addr;              \
 186                 __get8_unaligned_check(ins,v,a,err);            \
 187                 val =  v << ((BE) ? 24 :  0);                   \
 188                 __get8_unaligned_check(ins,v,a,err);            \
 189                 val |= v << ((BE) ? 16 :  8);                   \
 190                 __get8_unaligned_check(ins,v,a,err);            \
 191                 val |= v << ((BE) ?  8 : 16);                   \
 192                 __get8_unaligned_check(ins,v,a,err);            \
 193                 val |= v << ((BE) ?  0 : 24);                   \
 194                 if (err)                                        \
 195                         goto fault;                             \
 196         } while (0)
 197
 198 #define get32_unaligned_check(val,addr) \
 199         __get32_unaligned_check("ldrb",val,addr)
 200
 201 #define get32t_unaligned_check(val,addr) \
 202         __get32_unaligned_check("ldrbt",val,addr)
 203
 204 #define __put16_unaligned_check(ins,val,addr)                   \
 205         do {                                                    \
 206                 unsigned int err = 0, v = val, a = addr;        \
 207                 __asm__( FIRST_BYTE_16                          \
 208                 "1:     "ins"   %1, [%2], #1\n"                 \
 209                 "       mov     %1, %1, "NEXT_BYTE"\n"          \
 210                 "2:     "ins"   %1, [%2]\n"                     \
 211                 "3:\n"                                          \
 212                 "       .section .fixup,\"ax\"\n"               \
 213                 "       .align  2\n"                            \
 214                 "4:     mov     %0, #1\n"                       \
 215                 "       b       3b\n"                           \
 216                 "       .previous\n"                            \
 217                 "       .section __ex_table,\"a\"\n"            \
 218                 "       .align  3\n"                            \
 219                 "       .long   1b, 4b\n"                       \
 220                 "       .long   2b, 4b\n"                       \
 221                 "       .previous\n"                            \
 222                 : "=r" (err), "=&r" (v), "=&r" (a)              \
 223                 : "0" (err), "1" (v), "2" (a));                 \
 224                 if (err)                                        \
 225                         goto fault;                             \
 226         } while (0)
 227
 228 #define put16_unaligned_check(val,addr)  \
 229         __put16_unaligned_check("strb",val,addr)
 230
 231 #define put16t_unaligned_check(val,addr) \
 232         __put16_unaligned_check("strbt",val,addr)
 233
 234 #define __put32_unaligned_check(ins,val,addr)                   \
 235         do {                                                    \
 236                 unsigned int err = 0, v = val, a = addr;        \
 237                 __asm__( FIRST_BYTE_32                          \
 238                 "1:     "ins"   %1, [%2], #1\n"                 \
 239                 "       mov     %1, %1, "NEXT_BYTE"\n"          \
 240                 "2:     "ins"   %1, [%2], #1\n"                 \
 241                 "       mov     %1, %1, "NEXT_BYTE"\n"          \
 242                 "3:     "ins"   %1, [%2], #1\n"                 \
 243                 "       mov     %1, %1, "NEXT_BYTE"\n"          \
 244                 "4:     "ins"   %1, [%2]\n"                     \
 245                 "5:\n"                                          \
 246                 "       .section .fixup,\"ax\"\n"               \
 247                 "       .align  2\n"                            \
 248                 "6:     mov     %0, #1\n"                       \
 249                 "       b       5b\n"                           \
 250                 "       .previous\n"                            \
 251                 "       .section __ex_table,\"a\"\n"            \
 252                 "       .align  3\n"                            \
 253                 "       .long   1b, 6b\n"                       \
 254                 "       .long   2b, 6b\n"                       \
 255                 "       .long   3b, 6b\n"                       \
 256                 "       .long   4b, 6b\n"                       \
 257                 "       .previous\n"                            \
 258                 : "=r" (err), "=&r" (v), "=&r" (a)              \
 259                 : "0" (err), "1" (v), "2" (a));                 \
 260                 if (err)                                        \
 261                         goto fault;                             \
 262         } while (0)
 263
 264 #define put32_unaligned_check(val,addr) \
 265         __put32_unaligned_check("strb", val, addr)
 266
 267 #define put32t_unaligned_check(val,addr) \
 268         __put32_unaligned_check("strbt", val, addr)
 269
 270 static void
 271 do_alignment_finish_ldst(unsigned long addr, unsigned long instr, struct pt_regs *regs, union offset_union offset)
 272 {
 273         if (!LDST_U_BIT(instr))
 274                 offset.un = -offset.un;
 275
 276         if (!LDST_P_BIT(instr))
 277                 addr += offset.un;
 278
 279         if (!LDST_P_BIT(instr) || LDST_W_BIT(instr))
 280                 regs->uregs[RN_BITS(instr)] = addr;
 281 }
 282
 283 static int
 284 do_alignment_ldrhstrh(unsigned long addr, unsigned long instr, struct pt_regs *regs)
 285 {
 286         unsigned int rd = RD_BITS(instr);
 287
 288         ai_half += 1;
 289
 290         if (user_mode(regs))
 291                 goto user;
 292
 293         if (LDST_L_BIT(instr)) {
 294                 unsigned long val;
 295                 get16_unaligned_check(val, addr);
 296
 297                 /* signed half-word? */
 298                 if (instr & 0x40)
 299                         val = (signed long)((signed short) val);
 300
 301                 regs->uregs[rd] = val;
 302         } else
 303                 put16_unaligned_check(regs->uregs[rd], addr);
 304
 305         return TYPE_LDST;
 306
 307  user:
 308         if (LDST_L_BIT(instr)) {
 309                 unsigned long val;
 310                 get16t_unaligned_check(val, addr);
 311
 312                 /* signed half-word? */
 313                 if (instr & 0x40)
 314                         val = (signed long)((signed short) val);
 315
 316                 regs->uregs[rd] = val;
 317         } else
 318                 put16t_unaligned_check(regs->uregs[rd], addr);
 319
 320         return TYPE_LDST;
 321
 322  fault:
 323         return TYPE_FAULT;
 324 }
 325
 326 static int
 327 do_alignment_ldrdstrd(unsigned long addr, unsigned long instr,
 328                       struct pt_regs *regs)
 329 {
 330         unsigned int rd = RD_BITS(instr);
 331
 332         if (((rd & 1) == 1) || (rd == 14))
 333                 goto bad;
 334
 335         ai_dword += 1;
 336
 337         if (user_mode(regs))
 338                 goto user;
 339
 340         if ((instr & 0xf0) == 0xd0) {
 341                 unsigned long val;
 342                 get32_unaligned_check(val, addr);
 343                 regs->uregs[rd] = val;
 344                 get32_unaligned_check(val, addr + 4);
 345                 regs->uregs[rd + 1] = val;
 346         } else {
 347                 put32_unaligned_check(regs->uregs[rd], addr);
 348                 put32_unaligned_check(regs->uregs[rd + 1], addr + 4);
 349         }
 350
 351         return TYPE_LDST;
 352
 353  user:
 354         if ((instr & 0xf0) == 0xd0) {
 355                 unsigned long val;
 356                 get32t_unaligned_check(val, addr);
 357                 regs->uregs[rd] = val;
 358                 get32t_unaligned_check(val, addr + 4);
 359                 regs->uregs[rd + 1] = val;
 360         } else {
 361                 put32t_unaligned_check(regs->uregs[rd], addr);
 362                 put32t_unaligned_check(regs->uregs[rd + 1], addr + 4);
 363         }
 364
 365         return TYPE_LDST;
 366  bad:
 367         return TYPE_ERROR;
 368  fault:
 369         return TYPE_FAULT;
 370 }
 371
 372 static int
 373 do_alignment_ldrstr(unsigned long addr, unsigned long instr, struct pt_regs *regs)
 374 {
 375         unsigned int rd = RD_BITS(instr);
 376
 377         ai_word += 1;
 378
 379         if ((!LDST_P_BIT(instr) && LDST_W_BIT(instr)) || user_mode(regs))
 380                 goto trans;
 381
 382         if (LDST_L_BIT(instr)) {
 383                 unsigned int val;
 384                 get32_unaligned_check(val, addr);
 385                 regs->uregs[rd] = val;
 386         } else
 387                 put32_unaligned_check(regs->uregs[rd], addr);
 388         return TYPE_LDST;
 389
 390  trans:
 391         if (LDST_L_BIT(instr)) {
 392                 unsigned int val;
 393                 get32t_unaligned_check(val, addr);
 394                 regs->uregs[rd] = val;
 395         } else
 396                 put32t_unaligned_check(regs->uregs[rd], addr);
 397         return TYPE_LDST;
 398
 399  fault:
 400         return TYPE_FAULT;
 401 }
 402
 403 /*
 404  * LDM/STM alignment handler.
 405  *
 406  * There are 4 variants of this instruction:
 407  *
 408  * B = rn pointer before instruction, A = rn pointer after instruction
 409  *              ------ increasing address ----->
 410  *              |    | r0 | r1 | ... | rx |    |
 411  * PU = 01             B                    A
 412  * PU = 11        B                    A
 413  * PU = 00        A                    B
 414  * PU = 10             A                    B
 415  */
 416 static int
 417 do_alignment_ldmstm(unsigned long addr, unsigned long instr, struct pt_regs *regs)
 418 {
 419         unsigned int rd, rn, correction, nr_regs, regbits;
 420         unsigned long eaddr, newaddr;
 421
 422         if (LDM_S_BIT(instr))
 423                 goto bad;
 424
 425         correction = 4; /* processor implementation defined */
 426         regs->ARM_pc += correction;
 427
 428         ai_multi += 1;
 429
 430         /* count the number of registers in the mask to be transferred */
 431         nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
 432
 433         rn = RN_BITS(instr);
 434         newaddr = eaddr = regs->uregs[rn];
 435
 436         if (!LDST_U_BIT(instr))
 437                 nr_regs = -nr_regs;
 438         newaddr += nr_regs;
 439         if (!LDST_U_BIT(instr))
 440                 eaddr = newaddr;
 441
 442         if (LDST_P_EQ_U(instr)) /* U = P */
 443                 eaddr += 4;
 444
 445         /*
 446          * For alignment faults on the ARM922T/ARM920T the MMU  makes
 447          * the FSR (and hence addr) equal to the updated base address
 448          * of the multiple access rather than the restored value.
 449          * Switch this message off if we've got a ARM92[02], otherwise
 450          * [ls]dm alignment faults are noisy!
 451          */
 452 #if !(defined CONFIG_CPU_ARM922T)  && !(defined CONFIG_CPU_ARM920T)
 453         /*
 454          * This is a "hint" - we already have eaddr worked out by the
 455          * processor for us.
 456          */
 457         if (addr != eaddr) {
 458                 printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
 459                         "addr = %08lx, eaddr = %08lx\n",
 460                          instruction_pointer(regs), instr, addr, eaddr);
 461                 show_regs(regs);
 462         }
 463 #endif
 464
 465         if (user_mode(regs)) {
 466                 for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
 467                      regbits >>= 1, rd += 1)
 468                         if (regbits & 1) {
 469                                 if (LDST_L_BIT(instr)) {
 470                                         unsigned int val;
 471                                         get32t_unaligned_check(val, eaddr);
 472                                         regs->uregs[rd] = val;
 473                                 } else
 474                                         put32t_unaligned_check(regs->uregs[rd], eaddr);
 475                                 eaddr += 4;
 476                         }
 477         } else {
 478                 for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
 479                      regbits >>= 1, rd += 1)
 480                         if (regbits & 1) {
 481                                 if (LDST_L_BIT(instr)) {
 482                                         unsigned int val;
 483                                         get32_unaligned_check(val, eaddr);
 484                                         regs->uregs[rd] = val;
 485                                 } else
 486                                         put32_unaligned_check(regs->uregs[rd], eaddr);
 487                                 eaddr += 4;
 488                         }
 489         }
 490
 491         if (LDST_W_BIT(instr))
 492                 regs->uregs[rn] = newaddr;
 493         if (!LDST_L_BIT(instr) || !(REGMASK_BITS(instr) & (1 << 15)))
 494                 regs->ARM_pc -= correction;
 495         return TYPE_DONE;
 496
 497 fault:
 498         regs->ARM_pc -= correction;
 499         return TYPE_FAULT;
 500
 501 bad:
 502         printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n");
 503         return TYPE_ERROR;
 504 }
 505
 506 /*
 507  * Convert Thumb ld/st instruction forms to equivalent ARM instructions so
 508  * we can reuse ARM userland alignment fault fixups for Thumb.
 509  *
 510  * This implementation was initially based on the algorithm found in
 511  * gdb/sim/arm/thumbemu.c. It is basically just a code reduction of same
 512  * to convert only Thumb ld/st instruction forms to equivalent ARM forms.
 513  *
 514  * NOTES:
 515  * 1. Comments below refer to ARM ARM DDI0100E Thumb Instruction sections.
 516  * 2. If for some reason we're passed an non-ld/st Thumb instruction to
 517  *    decode, we return 0xdeadc0de. This should never happen under normal
 518  *    circumstances but if it does, we've got other problems to deal with
 519  *    elsewhere and we obviously can't fix those problems here.
 520  */
 521
 522 static unsigned long
 523 thumb2arm(u16 tinstr)
 524 {
 525         u32 L = (tinstr & (1<<11)) >> 11;
 526
 527         switch ((tinstr & 0xf800) >> 11) {
 528         /* 6.5.1 Format 1: */
 529         case 0x6000 >> 11:                              /* 7.1.52 STR(1) */
 530         case 0x6800 >> 11:                              /* 7.1.26 LDR(1) */
 531         case 0x7000 >> 11:                              /* 7.1.55 STRB(1) */
 532         case 0x7800 >> 11:                              /* 7.1.30 LDRB(1) */
 533                 return 0xe5800000 |
 534                         ((tinstr & (1<<12)) << (22-12)) |       /* fixup */
 535                         (L<<20) |                               /* L==1? */
 536                         ((tinstr & (7<<0)) << (12-0)) |         /* Rd */
 537                         ((tinstr & (7<<3)) << (16-3)) |         /* Rn */
 538                         ((tinstr & (31<<6)) >>                  /* immed_5 */
 539                                 (6 - ((tinstr & (1<<12)) ? 0 : 2)));
 540         case 0x8000 >> 11:                              /* 7.1.57 STRH(1) */
 541         case 0x8800 >> 11:                              /* 7.1.32 LDRH(1) */
 542                 return 0xe1c000b0 |
 543                         (L<<20) |                               /* L==1? */
 544                         ((tinstr & (7<<0)) << (12-0)) |         /* Rd */
 545                         ((tinstr & (7<<3)) << (16-3)) |         /* Rn */
 546                         ((tinstr & (7<<6)) >> (6-1)) |   /* immed_5[2:0] */
 547                         ((tinstr & (3<<9)) >> (9-8));    /* immed_5[4:3] */
 548
 549         /* 6.5.1 Format 2: */
 550         case 0x5000 >> 11:
 551         case 0x5800 >> 11:
 552                 {
 553                         static const u32 subset[8] = {
 554                                 0xe7800000,             /* 7.1.53 STR(2) */
 555                                 0xe18000b0,             /* 7.1.58 STRH(2) */
 556                                 0xe7c00000,             /* 7.1.56 STRB(2) */
 557                                 0xe19000d0,             /* 7.1.34 LDRSB */
 558                                 0xe7900000,             /* 7.1.27 LDR(2) */
 559                                 0xe19000b0,             /* 7.1.33 LDRH(2) */
 560                                 0xe7d00000,             /* 7.1.31 LDRB(2) */
 561                                 0xe19000f0              /* 7.1.35 LDRSH */
 562                         };
 563                         return subset[(tinstr & (7<<9)) >> 9] |
 564                             ((tinstr & (7<<0)) << (12-0)) |     /* Rd */
 565                             ((tinstr & (7<<3)) << (16-3)) |     /* Rn */
 566                             ((tinstr & (7<<6)) >> (6-0));       /* Rm */
 567                 }
 568
 569         /* 6.5.1 Format 3: */
 570         case 0x4800 >> 11:                              /* 7.1.28 LDR(3) */
 571                 /* NOTE: This case is not technically possible. We're
 572                  *       loading 32-bit memory data via PC relative
 573                  *       addressing mode. So we can and should eliminate
 574                  *       this case. But I'll leave it here for now.
 575                  */
 576                 return 0xe59f0000 |
 577                     ((tinstr & (7<<8)) << (12-8)) |             /* Rd */
 578                     ((tinstr & 255) << (2-0));                  /* immed_8 */
 579
 580         /* 6.5.1 Format 4: */
 581         case 0x9000 >> 11:                              /* 7.1.54 STR(3) */
 582         case 0x9800 >> 11:                              /* 7.1.29 LDR(4) */
 583                 return 0xe58d0000 |
 584                         (L<<20) |                               /* L==1? */
 585                         ((tinstr & (7<<8)) << (12-8)) |         /* Rd */
 586                         ((tinstr & 255) << 2);                  /* immed_8 */
 587
 588         /* 6.6.1 Format 1: */
 589         case 0xc000 >> 11:                              /* 7.1.51 STMIA */
 590         case 0xc800 >> 11:                              /* 7.1.25 LDMIA */
 591                 {
 592                         u32 Rn = (tinstr & (7<<8)) >> 8;
 593                         u32 W = ((L<<Rn) & (tinstr&255)) ? 0 : 1<<21;
 594
 595                         return 0xe8800000 | W | (L<<20) | (Rn<<16) |
 596                                 (tinstr&255);
 597                 }
 598
 599         /* 6.6.1 Format 2: */
 600         case 0xb000 >> 11:                              /* 7.1.48 PUSH */
 601         case 0xb800 >> 11:                              /* 7.1.47 POP */
 602                 if ((tinstr & (3 << 9)) == 0x0400) {
 603                         static const u32 subset[4] = {
 604                                 0xe92d0000,     /* STMDB sp!,{registers} */
 605                                 0xe92d4000,     /* STMDB sp!,{registers,lr} */
 606                                 0xe8bd0000,     /* LDMIA sp!,{registers} */
 607                                 0xe8bd8000      /* LDMIA sp!,{registers,pc} */
 608                         };
 609                         return subset[(L<<1) | ((tinstr & (1<<8)) >> 8)] |
 610                             (tinstr & 255);             /* register_list */
 611                 }
 612                 /* Else fall through for illegal instruction case */
 613
 614         default:
 615                 return 0xdeadc0de;
 616         }
 617 }
 618
 619 static int
 620 do_alignment(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
 621 {
 622         union offset_union offset;
 623         unsigned long instr = 0, instrptr;
 624         int (*handler)(unsigned long addr, unsigned long instr, struct pt_regs *regs);
 625         unsigned int type;
 626         mm_segment_t fs;
 627         unsigned int fault;
 628         u16 tinstr = 0;
 629
 630         instrptr = instruction_pointer(regs);
 631
 632         fs = get_fs();
 633         set_fs(KERNEL_DS);
 634         if thumb_mode(regs) {
 635                 fault = __get_user(tinstr, (u16 *)(instrptr & ~1));
 636                 if (!(fault))
 637                         instr = thumb2arm(tinstr);
 638         } else
 639                 fault = __get_user(instr, (u32 *)instrptr);
 640         set_fs(fs);
 641
 642         if (fault) {
 643                 type = TYPE_FAULT;
 644                 goto bad_or_fault;
 645         }
 646
 647         if (user_mode(regs))
 648                 goto user;
 649
 650         ai_sys += 1;
 651
 652  fixup:
 653
 654         regs->ARM_pc += thumb_mode(regs) ? 2 : 4;
 655
 656         switch (CODING_BITS(instr)) {
 657         case 0x00000000:        /* 3.13.4 load/store instruction extensions */
 658                 if (LDSTHD_I_BIT(instr))
 659                         offset.un = (instr & 0xf00) >> 4 | (instr & 15);
 660                 else
 661                         offset.un = regs->uregs[RM_BITS(instr)];
 662
 663                 if ((instr & 0x000000f0) == 0x000000b0 || /* LDRH, STRH */
 664                     (instr & 0x001000f0) == 0x001000f0)   /* LDRSH */
 665                         handler = do_alignment_ldrhstrh;
 666                 else if ((instr & 0x001000f0) == 0x000000d0 || /* LDRD */
 667                          (instr & 0x001000f0) == 0x000000f0)   /* STRD */
 668                         handler = do_alignment_ldrdstrd;
 669                 else if ((instr & 0x01f00ff0) == 0x01000090) /* SWP */
 670                         goto swp;
 671                 else
 672                         goto bad;
 673                 break;
 674
 675         case 0x04000000:        /* ldr or str immediate */
 676                 offset.un = OFFSET_BITS(instr);
 677                 handler = do_alignment_ldrstr;
 678                 break;
 679
 680         case 0x06000000:        /* ldr or str register */
 681                 offset.un = regs->uregs[RM_BITS(instr)];
 682
 683                 if (IS_SHIFT(instr)) {
 684                         unsigned int shiftval = SHIFT_BITS(instr);
 685
 686                         switch(SHIFT_TYPE(instr)) {
 687                         case SHIFT_LSL:
 688                                 offset.un <<= shiftval;
 689                                 break;
 690
 691                         case SHIFT_LSR:
 692                                 offset.un >>= shiftval;
 693                                 break;
 694
 695                         case SHIFT_ASR:
 696                                 offset.sn >>= shiftval;
 697                                 break;
 698
 699                         case SHIFT_RORRRX:
 700                                 if (shiftval == 0) {
 701                                         offset.un >>= 1;
 702                                         if (regs->ARM_cpsr & PSR_C_BIT)
 703                                                 offset.un |= 1 << 31;
 704                                 } else
 705                                         offset.un = offset.un >> shiftval |
 706                                                           offset.un << (32 - shiftval);
 707                                 break;
 708                         }
 709                 }
 710                 handler = do_alignment_ldrstr;
 711                 break;
 712
 713         case 0x08000000:        /* ldm or stm */
 714                 handler = do_alignment_ldmstm;
 715                 break;
 716
 717         default:
 718                 goto bad;
 719         }
 720
 721         type = handler(addr, instr, regs);
 722
 723         if (type == TYPE_ERROR || type == TYPE_FAULT)
 724                 goto bad_or_fault;
 725
 726         if (type == TYPE_LDST)
 727                 do_alignment_finish_ldst(addr, instr, regs, offset);
 728
 729         return 0;
 730
 731  bad_or_fault:
 732         if (type == TYPE_ERROR)
 733                 goto bad;
 734         regs->ARM_pc -= thumb_mode(regs) ? 2 : 4;
 735         /*
 736          * We got a fault - fix it up, or die.
 737          */
 738         do_bad_area(current, current->mm, addr, fsr, regs);
 739         return 0;
 740
 741  swp:
 742         printk(KERN_ERR "Alignment trap: not handling swp instruction\n");
 743
 744  bad:
 745         /*
 746          * Oops, we didn't handle the instruction.
 747          */
 748         printk(KERN_ERR "Alignment trap: not handling instruction "
 749                 "%0*lx at [<%08lx>]\n",
 750                 thumb_mode(regs) ? 4 : 8,
 751                 thumb_mode(regs) ? tinstr : instr, instrptr);
 752         ai_skipped += 1;
 753         return 1;
 754
 755  user:
 756         ai_user += 1;
 757
 758         if (ai_usermode & 1)
 759                 printk("Alignment trap: %s (%d) PC=0x%08lx Instr=0x%0*lx "
 760                        "Address=0x%08lx FSR 0x%03x\n", current->comm,
 761                         current->pid, instrptr,
 762                         thumb_mode(regs) ? 4 : 8,
 763                         thumb_mode(regs) ? tinstr : instr,
 764                         addr, fsr);
 765
 766         if (ai_usermode & 2)
 767                 goto fixup;
 768
 769         if (ai_usermode & 4)
 770                 force_sig(SIGBUS, current);
 771         else
 772                 set_cr(cr_no_alignment);
 773
 774         return 0;
 775 }
 776
 777 /*
 778  * This needs to be done after sysctl_init, otherwise sys/ will be
 779  * overwritten.  Actually, this shouldn't be in sys/ at all since
 780  * it isn't a sysctl, and it doesn't contain sysctl information.
 781  * We now locate it in /proc/cpu/alignment instead.
 782  */
 783 static int __init alignment_init(void)
 784 {
 785 #ifdef CONFIG_PROC_FS
 786         struct proc_dir_entry *res;
 787
 788         res = proc_mkdir("cpu", NULL);
 789         if (!res)
 790                 return -ENOMEM;
 791
 792         res = create_proc_entry("alignment", S_IWUSR | S_IRUGO, res);
 793         if (!res)
 794                 return -ENOMEM;
 795
 796         res->read_proc = proc_alignment_read;
 797         res->write_proc = proc_alignment_write;
 798 #endif
 799
 800         hook_fault_code(1, do_alignment, SIGILL, "alignment exception");
 801         hook_fault_code(3, do_alignment, SIGILL, "alignment exception");
 802
 803         return 0;
 804 }
 805
 806 fs_initcall(alignment_init);