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 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]; |
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; |
71 | s16 imm16; |
72 | unsigned int fault; |
73 | |
74 | /* back up one instruction */ |
75 | fp->ea -= 4; |
76 | |
77 | if (fixup_exception(fp)) { |
78 | return; |
79 | } |
80 | |
81 | in_kernel = !user_mode(regs: 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, reg: 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, reg: b, val); |
102 | break; |
103 | case INST_STH: |
104 | val = get_reg_val(fp, reg: b); |
105 | d1 = val >> 8; |
106 | d0 = val >> 0; |
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 | } |
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, reg: b, val); |
120 | break; |
121 | case INST_STW: |
122 | val = get_reg_val(fp, reg: 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 | } |
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, reg: b, val); |
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) { |
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 | |
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); |
239 | |