Commit | Line | Data |
---|---|---|
82ed08dd LFT |
1 | /* |
2 | * linux/arch/nios2/kernel/misaligned.c | |
3 | * | |
4 | * basic emulation for mis-aligned accesses on the NIOS II cpu | |
5 | * modelled after the version for arm in arm/alignment.c | |
6 | * | |
7 | * Brad Parker <brad@heeltoe.com> | |
8 | * Copyright (C) 2010 Ambient Corporation | |
9 | * Copyright (c) 2010 Altera Corporation, San Jose, California, USA. | |
10 | * Copyright (c) 2010 Arrow Electronics, Inc. | |
11 | * | |
12 | * This file is subject to the terms and conditions of the GNU General | |
13 | * Public License. See the file COPYING in the main directory of | |
14 | * this archive for more details. | |
15 | */ | |
16 | ||
17 | #include <linux/errno.h> | |
18 | #include <linux/string.h> | |
19 | #include <linux/proc_fs.h> | |
20 | #include <linux/init.h> | |
21 | #include <linux/sched.h> | |
22 | #include <linux/uaccess.h> | |
23 | #include <linux/seq_file.h> | |
24 | ||
25 | #include <asm/traps.h> | |
26 | #include <asm/unaligned.h> | |
27 | ||
28 | /* instructions we emulate */ | |
29 | #define INST_LDHU 0x0b | |
30 | #define INST_STH 0x0d | |
31 | #define INST_LDH 0x0f | |
32 | #define INST_STW 0x15 | |
33 | #define INST_LDW 0x17 | |
34 | ||
35 | static unsigned long ma_user, ma_kern, ma_skipped, ma_half, ma_word; | |
36 | ||
37 | static unsigned int ma_usermode; | |
38 | #define UM_WARN 0x01 | |
39 | #define UM_FIXUP 0x02 | |
40 | #define UM_SIGNAL 0x04 | |
41 | #define KM_WARN 0x08 | |
42 | ||
43 | /* see arch/nios2/include/asm/ptrace.h */ | |
44 | static u8 sys_stack_frame_reg_offset[] = { | |
45 | /* struct pt_regs */ | |
46 | 8, 9, 10, 11, 12, 13, 14, 15, 1, 2, 3, 4, 5, 6, 7, 0, | |
47 | /* struct switch_stack */ | |
48 | 16, 17, 18, 19, 20, 21, 22, 23, 0, 0, 0, 0, 0, 0, 0, 0 | |
49 | }; | |
50 | ||
51 | static int reg_offsets[32]; | |
52 | ||
53 | static inline u32 get_reg_val(struct pt_regs *fp, int reg) | |
54 | { | |
55 | u8 *p = ((u8 *)fp) + reg_offsets[reg]; | |
56 | ||
57 | return *(u32 *)p; | |
58 | } | |
59 | ||
60 | static inline void put_reg_val(struct pt_regs *fp, int reg, u32 val) | |
61 | { | |
62 | u8 *p = ((u8 *)fp) + reg_offsets[reg]; | |
63 | *(u32 *)p = val; | |
64 | } | |
65 | ||
66 | /* | |
67 | * (mis)alignment handler | |
68 | */ | |
69 | asmlinkage void handle_unaligned_c(struct pt_regs *fp, int cause) | |
70 | { | |
71 | u32 isn, addr, val; | |
72 | int in_kernel; | |
73 | u8 a, b, d0, d1, d2, d3; | |
74 | u16 imm16; | |
75 | unsigned int fault; | |
76 | ||
77 | /* back up one instruction */ | |
78 | fp->ea -= 4; | |
79 | ||
80 | if (fixup_exception(fp)) { | |
81 | ma_skipped++; | |
82 | return; | |
83 | } | |
84 | ||
85 | in_kernel = !user_mode(fp); | |
86 | ||
87 | isn = *(unsigned long *)(fp->ea); | |
88 | ||
89 | fault = 0; | |
90 | ||
91 | /* do fixup if in kernel or mode turned on */ | |
92 | if (in_kernel || (ma_usermode & UM_FIXUP)) { | |
93 | /* decompose instruction */ | |
94 | a = (isn >> 27) & 0x1f; | |
95 | b = (isn >> 22) & 0x1f; | |
96 | imm16 = (isn >> 6) & 0xffff; | |
97 | addr = get_reg_val(fp, a) + imm16; | |
98 | ||
99 | /* do fixup to saved registers */ | |
100 | switch (isn & 0x3f) { | |
101 | case INST_LDHU: | |
102 | fault |= __get_user(d0, (u8 *)(addr+0)); | |
103 | fault |= __get_user(d1, (u8 *)(addr+1)); | |
104 | val = (d1 << 8) | d0; | |
105 | put_reg_val(fp, b, val); | |
106 | ma_half++; | |
107 | break; | |
108 | case INST_STH: | |
109 | val = get_reg_val(fp, b); | |
110 | d1 = val >> 8; | |
111 | d0 = val >> 0; | |
112 | ||
113 | pr_debug("sth: ra=%d (%08x) rb=%d (%08x), imm16 %04x addr %08x val %08x\n", | |
114 | a, get_reg_val(fp, a), | |
115 | b, get_reg_val(fp, b), | |
116 | imm16, addr, val); | |
117 | ||
118 | if (in_kernel) { | |
119 | *(u8 *)(addr+0) = d0; | |
120 | *(u8 *)(addr+1) = d1; | |
121 | } else { | |
122 | fault |= __put_user(d0, (u8 *)(addr+0)); | |
123 | fault |= __put_user(d1, (u8 *)(addr+1)); | |
124 | } | |
125 | ma_half++; | |
126 | break; | |
127 | case INST_LDH: | |
128 | fault |= __get_user(d0, (u8 *)(addr+0)); | |
129 | fault |= __get_user(d1, (u8 *)(addr+1)); | |
130 | val = (short)((d1 << 8) | d0); | |
131 | put_reg_val(fp, b, val); | |
132 | ma_half++; | |
133 | break; | |
134 | case INST_STW: | |
135 | val = get_reg_val(fp, b); | |
136 | d3 = val >> 24; | |
137 | d2 = val >> 16; | |
138 | d1 = val >> 8; | |
139 | d0 = val >> 0; | |
140 | if (in_kernel) { | |
141 | *(u8 *)(addr+0) = d0; | |
142 | *(u8 *)(addr+1) = d1; | |
143 | *(u8 *)(addr+2) = d2; | |
144 | *(u8 *)(addr+3) = d3; | |
145 | } else { | |
146 | fault |= __put_user(d0, (u8 *)(addr+0)); | |
147 | fault |= __put_user(d1, (u8 *)(addr+1)); | |
148 | fault |= __put_user(d2, (u8 *)(addr+2)); | |
149 | fault |= __put_user(d3, (u8 *)(addr+3)); | |
150 | } | |
151 | ma_word++; | |
152 | break; | |
153 | case INST_LDW: | |
154 | fault |= __get_user(d0, (u8 *)(addr+0)); | |
155 | fault |= __get_user(d1, (u8 *)(addr+1)); | |
156 | fault |= __get_user(d2, (u8 *)(addr+2)); | |
157 | fault |= __get_user(d3, (u8 *)(addr+3)); | |
158 | val = (d3 << 24) | (d2 << 16) | (d1 << 8) | d0; | |
159 | put_reg_val(fp, b, val); | |
160 | ma_word++; | |
161 | break; | |
162 | } | |
163 | } | |
164 | ||
165 | addr = RDCTL(CTL_BADADDR); | |
166 | cause >>= 2; | |
167 | ||
168 | if (fault) { | |
169 | if (in_kernel) { | |
170 | pr_err("fault during kernel misaligned fixup @ %#lx; addr 0x%08x; isn=0x%08x\n", | |
171 | fp->ea, (unsigned int)addr, | |
172 | (unsigned int)isn); | |
173 | } else { | |
174 | pr_err("fault during user misaligned fixup @ %#lx; isn=%08x addr=0x%08x sp=0x%08lx pid=%d\n", | |
175 | fp->ea, | |
176 | (unsigned int)isn, addr, fp->sp, | |
177 | current->pid); | |
178 | ||
179 | _exception(SIGSEGV, fp, SEGV_MAPERR, fp->ea); | |
180 | return; | |
181 | } | |
182 | } | |
183 | ||
184 | /* | |
185 | * kernel mode - | |
186 | * note exception and skip bad instruction (return) | |
187 | */ | |
188 | if (in_kernel) { | |
189 | ma_kern++; | |
190 | fp->ea += 4; | |
191 | ||
192 | if (ma_usermode & KM_WARN) { | |
193 | pr_err("kernel unaligned access @ %#lx; BADADDR 0x%08x; cause=%d, isn=0x%08x\n", | |
194 | fp->ea, | |
195 | (unsigned int)addr, cause, | |
196 | (unsigned int)isn); | |
197 | /* show_regs(fp); */ | |
198 | } | |
199 | ||
200 | return; | |
201 | } | |
202 | ||
203 | ma_user++; | |
204 | ||
205 | /* | |
206 | * user mode - | |
207 | * possibly warn, | |
208 | * possibly send SIGBUS signal to process | |
209 | */ | |
210 | if (ma_usermode & UM_WARN) { | |
211 | pr_err("user unaligned access @ %#lx; isn=0x%08lx ea=0x%08lx ra=0x%08lx sp=0x%08lx\n", | |
212 | (unsigned long)addr, (unsigned long)isn, | |
213 | fp->ea, fp->ra, fp->sp); | |
214 | } | |
215 | ||
216 | if (ma_usermode & UM_SIGNAL) | |
217 | _exception(SIGBUS, fp, BUS_ADRALN, fp->ea); | |
218 | else | |
219 | fp->ea += 4; /* else advance */ | |
220 | } | |
221 | ||
222 | static void __init misaligned_calc_reg_offsets(void) | |
223 | { | |
224 | int i, r, offset; | |
225 | ||
226 | /* pre-calc offsets of registers on sys call stack frame */ | |
227 | offset = 0; | |
228 | ||
229 | /* struct pt_regs */ | |
230 | for (i = 0; i < 16; i++) { | |
231 | r = sys_stack_frame_reg_offset[i]; | |
232 | reg_offsets[r] = offset; | |
233 | offset += 4; | |
234 | } | |
235 | ||
236 | /* struct switch_stack */ | |
237 | offset = -sizeof(struct switch_stack); | |
238 | for (i = 16; i < 32; i++) { | |
239 | r = sys_stack_frame_reg_offset[i]; | |
240 | reg_offsets[r] = offset; | |
241 | offset += 4; | |
242 | } | |
243 | } | |
244 | ||
245 | ||
246 | static int __init misaligned_init(void) | |
247 | { | |
248 | /* default mode - silent fix */ | |
249 | ma_usermode = UM_FIXUP | KM_WARN; | |
250 | ||
251 | misaligned_calc_reg_offsets(); | |
252 | ||
253 | return 0; | |
254 | } | |
255 | ||
256 | fs_initcall(misaligned_init); |