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