2 * This file is subject to the terms and conditions of the GNU General Public
3 * License. See the file "COPYING" in the main directory of this archive
6 * Copyright (C) 2014 Lemote Corporation.
7 * written by Huacai Chen <chenhc@lemote.com>
9 * based on arch/mips/cavium-octeon/cpu.c
10 * Copyright (C) 2009 Wind River Systems,
11 * written by Ralf Baechle <ralf@linux-mips.org>
13 #include <linux/init.h>
14 #include <linux/sched.h>
15 #include <linux/notifier.h>
16 #include <linux/ptrace.h>
17 #include <linux/uaccess.h>
18 #include <linux/sched/signal.h>
23 #include <asm/branch.h>
24 #include <asm/current.h>
25 #include <asm/mipsregs.h>
26 #include <asm/unaligned-emul.h>
28 static int loongson_cu2_call(struct notifier_block *nfb, unsigned long action,
31 unsigned int res, fpu_owned;
32 unsigned long ra, value, value_next;
33 union mips_instruction insn;
34 int fr = !test_thread_flag(TIF_32BIT_FPREGS);
35 struct pt_regs *regs = (struct pt_regs *)data;
36 void __user *addr = (void __user *)regs->cp0_badvaddr;
37 unsigned int __user *pc = (unsigned int __user *)exception_epc(regs);
40 __get_user(insn.word, pc);
45 fpu_owned = __is_fpu_owner();
47 set_c0_status(ST0_CU1 | ST0_CU2);
49 set_c0_status(ST0_CU1 | ST0_CU2 | ST0_FR);
51 KSTK_STATUS(current) |= (ST0_CU1 | ST0_CU2);
53 KSTK_STATUS(current) |= ST0_FR;
55 KSTK_STATUS(current) &= ~ST0_FR;
56 /* If FPU is owned, we needn't init or restore fp */
58 set_thread_flag(TIF_USEDFPU);
64 return NOTIFY_STOP; /* Don't call default notifier */
67 if (insn.loongson3_lswc2_format.ls == 0)
70 if (insn.loongson3_lswc2_format.fr == 0) { /* gslq */
71 if (!access_ok(addr, 16))
74 LoadDW(addr, value, res);
78 LoadDW(addr + 8, value_next, res);
82 regs->regs[insn.loongson3_lswc2_format.rt] = value;
83 regs->regs[insn.loongson3_lswc2_format.rq] = value_next;
84 compute_return_epc(regs);
86 if (!access_ok(addr, 16))
90 LoadDW(addr, value, res);
94 LoadDW(addr + 8, value_next, res);
98 set_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lswc2_format.rt], 0, value);
99 set_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lswc2_format.rq], 0, value_next);
100 compute_return_epc(regs);
103 return NOTIFY_STOP; /* Don't call default notifier */
106 if (insn.loongson3_lswc2_format.ls == 0)
109 if (insn.loongson3_lswc2_format.fr == 0) { /* gssq */
110 if (!access_ok(addr, 16))
113 /* write upper 8 bytes first */
114 value_next = regs->regs[insn.loongson3_lswc2_format.rq];
116 StoreDW(addr + 8, value_next, res);
119 value = regs->regs[insn.loongson3_lswc2_format.rt];
121 StoreDW(addr, value, res);
125 compute_return_epc(regs);
126 } else { /* gssqc1 */
127 if (!access_ok(addr, 16))
131 value_next = get_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lswc2_format.rq], 0);
133 StoreDW(addr + 8, value_next, res);
137 value = get_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lswc2_format.rt], 0);
139 StoreDW(addr, value, res);
143 compute_return_epc(regs);
146 return NOTIFY_STOP; /* Don't call default notifier */
149 switch (insn.loongson3_lsdc2_format.opcode1) {
151 * Loongson-3 overridden ldc2 instructions.
152 * opcode1 instruction
153 * 0x1 gslhx: load 2 bytes to GPR
154 * 0x2 gslwx: load 4 bytes to GPR
155 * 0x3 gsldx: load 8 bytes to GPR
156 * 0x6 gslwxc1: load 4 bytes to FPR
157 * 0x7 gsldxc1: load 8 bytes to FPR
160 if (!access_ok(addr, 2))
163 LoadHW(addr, value, res);
167 compute_return_epc(regs);
168 regs->regs[insn.loongson3_lsdc2_format.rt] = value;
171 if (!access_ok(addr, 4))
174 LoadW(addr, value, res);
178 compute_return_epc(regs);
179 regs->regs[insn.loongson3_lsdc2_format.rt] = value;
182 if (!access_ok(addr, 8))
185 LoadDW(addr, value, res);
189 compute_return_epc(regs);
190 regs->regs[insn.loongson3_lsdc2_format.rt] = value;
193 die_if_kernel("Unaligned FP access in kernel code", regs);
194 BUG_ON(!used_math());
195 if (!access_ok(addr, 4))
199 LoadW(addr, value, res);
203 set_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lsdc2_format.rt], 0, value);
204 compute_return_epc(regs);
209 die_if_kernel("Unaligned FP access in kernel code", regs);
210 BUG_ON(!used_math());
211 if (!access_ok(addr, 8))
215 LoadDW(addr, value, res);
219 set_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lsdc2_format.rt], 0, value);
220 compute_return_epc(regs);
225 return NOTIFY_STOP; /* Don't call default notifier */
228 switch (insn.loongson3_lsdc2_format.opcode1) {
230 * Loongson-3 overridden sdc2 instructions.
231 * opcode1 instruction
232 * 0x1 gsshx: store 2 bytes from GPR
233 * 0x2 gsswx: store 4 bytes from GPR
234 * 0x3 gssdx: store 8 bytes from GPR
235 * 0x6 gsswxc1: store 4 bytes from FPR
236 * 0x7 gssdxc1: store 8 bytes from FPR
239 if (!access_ok(addr, 2))
242 compute_return_epc(regs);
243 value = regs->regs[insn.loongson3_lsdc2_format.rt];
245 StoreHW(addr, value, res);
251 if (!access_ok(addr, 4))
254 compute_return_epc(regs);
255 value = regs->regs[insn.loongson3_lsdc2_format.rt];
257 StoreW(addr, value, res);
263 if (!access_ok(addr, 8))
266 compute_return_epc(regs);
267 value = regs->regs[insn.loongson3_lsdc2_format.rt];
269 StoreDW(addr, value, res);
276 die_if_kernel("Unaligned FP access in kernel code", regs);
277 BUG_ON(!used_math());
279 if (!access_ok(addr, 4))
283 value = get_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lsdc2_format.rt], 0);
285 StoreW(addr, value, res);
289 compute_return_epc(regs);
294 die_if_kernel("Unaligned FP access in kernel code", regs);
295 BUG_ON(!used_math());
297 if (!access_ok(addr, 8))
301 value = get_fpr64(¤t->thread.fpu.fpr[insn.loongson3_lsdc2_format.rt], 0);
303 StoreDW(addr, value, res);
307 compute_return_epc(regs);
312 return NOTIFY_STOP; /* Don't call default notifier */
315 return NOTIFY_OK; /* Let default notifier send signals */
318 /* roll back jump/branch */
320 regs->cp0_epc = (unsigned long)pc;
321 /* Did we have an exception handler installed? */
322 if (fixup_exception(regs))
323 return NOTIFY_STOP; /* Don't call default notifier */
325 die_if_kernel("Unhandled kernel unaligned access", regs);
328 return NOTIFY_STOP; /* Don't call default notifier */
331 die_if_kernel("Unhandled kernel unaligned access", regs);
334 return NOTIFY_STOP; /* Don't call default notifier */
337 static int __init loongson_cu2_setup(void)
339 return cu2_notifier(loongson_cu2_call, 0);
341 early_initcall(loongson_cu2_setup);