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 | ||
82ed08dd LFT |
35 | static unsigned int ma_usermode; |
36 | #define UM_WARN 0x01 | |
37 | #define UM_FIXUP 0x02 | |
38 | #define UM_SIGNAL 0x04 | |
39 | #define KM_WARN 0x08 | |
40 | ||
41 | /* see arch/nios2/include/asm/ptrace.h */ | |
42 | static u8 sys_stack_frame_reg_offset[] = { | |
43 | /* struct pt_regs */ | |
44 | 8, 9, 10, 11, 12, 13, 14, 15, 1, 2, 3, 4, 5, 6, 7, 0, | |
45 | /* struct switch_stack */ | |
46 | 16, 17, 18, 19, 20, 21, 22, 23, 0, 0, 0, 0, 0, 0, 0, 0 | |
47 | }; | |
48 | ||
49 | static int reg_offsets[32]; | |
50 | ||
51 | static inline u32 get_reg_val(struct pt_regs *fp, int reg) | |
52 | { | |
53 | u8 *p = ((u8 *)fp) + reg_offsets[reg]; | |
82ed08dd LFT |
54 | return *(u32 *)p; |
55 | } | |
56 | ||
57 | static inline void put_reg_val(struct pt_regs *fp, int reg, u32 val) | |
58 | { | |
59 | u8 *p = ((u8 *)fp) + reg_offsets[reg]; | |
60 | *(u32 *)p = val; | |
61 | } | |
62 | ||
63 | /* | |
64 | * (mis)alignment handler | |
65 | */ | |
66 | asmlinkage void handle_unaligned_c(struct pt_regs *fp, int cause) | |
67 | { | |
68 | u32 isn, addr, val; | |
69 | int in_kernel; | |
70 | u8 a, b, d0, d1, d2, d3; | |
db5a7e55 | 71 | s16 imm16; |
82ed08dd LFT |
72 | unsigned int fault; |
73 | ||
74 | /* back up one instruction */ | |
75 | fp->ea -= 4; | |
76 | ||
77 | if (fixup_exception(fp)) { | |
82ed08dd LFT |
78 | return; |
79 | } | |
80 | ||
81 | in_kernel = !user_mode(fp); | |
82 | ||
83 | isn = *(unsigned long *)(fp->ea); | |
84 | ||
85 | fault = 0; | |
86 | ||
87 | /* do fixup if in kernel or mode turned on */ | |
88 | if (in_kernel || (ma_usermode & UM_FIXUP)) { | |
89 | /* decompose instruction */ | |
90 | a = (isn >> 27) & 0x1f; | |
91 | b = (isn >> 22) & 0x1f; | |
92 | imm16 = (isn >> 6) & 0xffff; | |
93 | addr = get_reg_val(fp, a) + imm16; | |
94 | ||
95 | /* do fixup to saved registers */ | |
96 | switch (isn & 0x3f) { | |
97 | case INST_LDHU: | |
98 | fault |= __get_user(d0, (u8 *)(addr+0)); | |
99 | fault |= __get_user(d1, (u8 *)(addr+1)); | |
100 | val = (d1 << 8) | d0; | |
101 | put_reg_val(fp, b, val); | |
82ed08dd LFT |
102 | break; |
103 | case INST_STH: | |
104 | val = get_reg_val(fp, b); | |
105 | d1 = val >> 8; | |
106 | d0 = val >> 0; | |
82ed08dd LFT |
107 | if (in_kernel) { |
108 | *(u8 *)(addr+0) = d0; | |
109 | *(u8 *)(addr+1) = d1; | |
110 | } else { | |
111 | fault |= __put_user(d0, (u8 *)(addr+0)); | |
112 | fault |= __put_user(d1, (u8 *)(addr+1)); | |
113 | } | |
82ed08dd LFT |
114 | break; |
115 | case INST_LDH: | |
116 | fault |= __get_user(d0, (u8 *)(addr+0)); | |
117 | fault |= __get_user(d1, (u8 *)(addr+1)); | |
118 | val = (short)((d1 << 8) | d0); | |
119 | put_reg_val(fp, b, val); | |
82ed08dd LFT |
120 | break; |
121 | case INST_STW: | |
122 | val = get_reg_val(fp, b); | |
123 | d3 = val >> 24; | |
124 | d2 = val >> 16; | |
125 | d1 = val >> 8; | |
126 | d0 = val >> 0; | |
127 | if (in_kernel) { | |
128 | *(u8 *)(addr+0) = d0; | |
129 | *(u8 *)(addr+1) = d1; | |
130 | *(u8 *)(addr+2) = d2; | |
131 | *(u8 *)(addr+3) = d3; | |
132 | } else { | |
133 | fault |= __put_user(d0, (u8 *)(addr+0)); | |
134 | fault |= __put_user(d1, (u8 *)(addr+1)); | |
135 | fault |= __put_user(d2, (u8 *)(addr+2)); | |
136 | fault |= __put_user(d3, (u8 *)(addr+3)); | |
137 | } | |
82ed08dd LFT |
138 | break; |
139 | case INST_LDW: | |
140 | fault |= __get_user(d0, (u8 *)(addr+0)); | |
141 | fault |= __get_user(d1, (u8 *)(addr+1)); | |
142 | fault |= __get_user(d2, (u8 *)(addr+2)); | |
143 | fault |= __get_user(d3, (u8 *)(addr+3)); | |
144 | val = (d3 << 24) | (d2 << 16) | (d1 << 8) | d0; | |
145 | put_reg_val(fp, b, val); | |
82ed08dd LFT |
146 | break; |
147 | } | |
148 | } | |
149 | ||
150 | addr = RDCTL(CTL_BADADDR); | |
151 | cause >>= 2; | |
152 | ||
153 | if (fault) { | |
154 | if (in_kernel) { | |
155 | pr_err("fault during kernel misaligned fixup @ %#lx; addr 0x%08x; isn=0x%08x\n", | |
156 | fp->ea, (unsigned int)addr, | |
157 | (unsigned int)isn); | |
158 | } else { | |
159 | pr_err("fault during user misaligned fixup @ %#lx; isn=%08x addr=0x%08x sp=0x%08lx pid=%d\n", | |
160 | fp->ea, | |
161 | (unsigned int)isn, addr, fp->sp, | |
162 | current->pid); | |
163 | ||
164 | _exception(SIGSEGV, fp, SEGV_MAPERR, fp->ea); | |
165 | return; | |
166 | } | |
167 | } | |
168 | ||
169 | /* | |
170 | * kernel mode - | |
171 | * note exception and skip bad instruction (return) | |
172 | */ | |
173 | if (in_kernel) { | |
82ed08dd LFT |
174 | fp->ea += 4; |
175 | ||
176 | if (ma_usermode & KM_WARN) { | |
177 | pr_err("kernel unaligned access @ %#lx; BADADDR 0x%08x; cause=%d, isn=0x%08x\n", | |
178 | fp->ea, | |
179 | (unsigned int)addr, cause, | |
180 | (unsigned int)isn); | |
181 | /* show_regs(fp); */ | |
182 | } | |
183 | ||
184 | return; | |
185 | } | |
186 | ||
82ed08dd LFT |
187 | /* |
188 | * user mode - | |
189 | * possibly warn, | |
190 | * possibly send SIGBUS signal to process | |
191 | */ | |
192 | if (ma_usermode & UM_WARN) { | |
193 | pr_err("user unaligned access @ %#lx; isn=0x%08lx ea=0x%08lx ra=0x%08lx sp=0x%08lx\n", | |
194 | (unsigned long)addr, (unsigned long)isn, | |
195 | fp->ea, fp->ra, fp->sp); | |
196 | } | |
197 | ||
198 | if (ma_usermode & UM_SIGNAL) | |
199 | _exception(SIGBUS, fp, BUS_ADRALN, fp->ea); | |
200 | else | |
201 | fp->ea += 4; /* else advance */ | |
202 | } | |
203 | ||
204 | static void __init misaligned_calc_reg_offsets(void) | |
205 | { | |
206 | int i, r, offset; | |
207 | ||
208 | /* pre-calc offsets of registers on sys call stack frame */ | |
209 | offset = 0; | |
210 | ||
211 | /* struct pt_regs */ | |
212 | for (i = 0; i < 16; i++) { | |
213 | r = sys_stack_frame_reg_offset[i]; | |
214 | reg_offsets[r] = offset; | |
215 | offset += 4; | |
216 | } | |
217 | ||
218 | /* struct switch_stack */ | |
219 | offset = -sizeof(struct switch_stack); | |
220 | for (i = 16; i < 32; i++) { | |
221 | r = sys_stack_frame_reg_offset[i]; | |
222 | reg_offsets[r] = offset; | |
223 | offset += 4; | |
224 | } | |
225 | } | |
226 | ||
227 | ||
228 | static int __init misaligned_init(void) | |
229 | { | |
230 | /* default mode - silent fix */ | |
231 | ma_usermode = UM_FIXUP | KM_WARN; | |
232 | ||
233 | misaligned_calc_reg_offsets(); | |
234 | ||
235 | return 0; | |
236 | } | |
237 | ||
238 | fs_initcall(misaligned_init); |