[PATCH v3 05/33] nds32: Exception handling

From: Greentime Hu
Date: Fri Dec 08 2017 - 05:02:33 EST


From: Greentime Hu <greentime@xxxxxxxxxxxxx>

This patch includes the exception/interrupt entries, pt_reg structure and
related accessors.

Signed-off-by: Vincent Chen <vincentc@xxxxxxxxxxxxx>
Signed-off-by: Greentime Hu <greentime@xxxxxxxxxxxxx>
---
arch/nds32/include/asm/ptrace.h | 79 +++++
arch/nds32/kernel/ex-entry.S | 170 +++++++++++
arch/nds32/kernel/ex-exit.S | 206 +++++++++++++
arch/nds32/kernel/stacktrace.c | 60 ++++
arch/nds32/kernel/traps.c | 441 +++++++++++++++++++++++++++
arch/nds32/mm/alignment.c | 622 +++++++++++++++++++++++++++++++++++++++
6 files changed, 1578 insertions(+)
create mode 100644 arch/nds32/include/asm/ptrace.h
create mode 100644 arch/nds32/kernel/ex-entry.S
create mode 100644 arch/nds32/kernel/ex-exit.S
create mode 100644 arch/nds32/kernel/stacktrace.c
create mode 100644 arch/nds32/kernel/traps.c
create mode 100644 arch/nds32/mm/alignment.c

diff --git a/arch/nds32/include/asm/ptrace.h b/arch/nds32/include/asm/ptrace.h
new file mode 100644
index 0000000..2c9c03d
--- /dev/null
+++ b/arch/nds32/include/asm/ptrace.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) 2005-2017 Andes Technology Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ASM_NDS32_PTRACE_H
+#define __ASM_NDS32_PTRACE_H
+
+#define PTRACE_GETREGS 12
+#define PTRACE_SETREGS 13
+#define PTRACE_GETFPREGS 14
+#define PTRACE_SETFPREGS 15
+
+#include <uapi/asm/ptrace.h>
+
+#ifndef __ASSEMBLY__
+
+struct pt_regs {
+ union {
+ struct user_pt_regs user_regs;
+ struct {
+ long uregs[26];
+ long fp;
+ long gp;
+ long lp;
+ long sp;
+ long ipc;
+#if defined(CONFIG_HWZOL)
+ long lb;
+ long le;
+ long lc;
+#else
+ long dummy[3];
+#endif
+ long syscallno;
+ };
+ };
+ long orig_r0;
+ long ir0;
+ long ipsw;
+ long pipsw;
+ long pipc;
+ long pp0;
+ long pp1;
+ long fucop_ctl;
+ long osp;
+};
+
+#include <asm/bitfield.h>
+extern void show_regs(struct pt_regs *);
+/* Avoid circular header include via sched.h */
+struct task_struct;
+extern void send_sigtrap(struct task_struct *tsk, struct pt_regs *regs,
+ int error_code, int si_code);
+
+#define arch_has_single_step() (1)
+#define user_mode(regs) (((regs)->ipsw & PSW_mskPOM) == 0)
+#define interrupts_enabled(regs) (!!((regs)->ipsw & PSW_mskGIE))
+#define valid_user_regs(regs) (user_mode(regs) && interrupts_enabled(regs))
+#define regs_return_value(regs) ((regs)->uregs[0])
+#define instruction_pointer(regs) ((regs)->ipc)
+#define user_stack_pointer(regs) ((regs)->sp)
+#define profile_pc(regs) instruction_pointer(regs)
+
+#define ARCH_HAS_USER_SINGLE_STEP_INFO
+
+#endif /* __ASSEMBLY__ */
+#endif
diff --git a/arch/nds32/kernel/ex-entry.S b/arch/nds32/kernel/ex-entry.S
new file mode 100644
index 0000000..09dc6a0
--- /dev/null
+++ b/arch/nds32/kernel/ex-entry.S
@@ -0,0 +1,170 @@
+/*
+ * Copyright (C) 2005-2017 Andes Technology Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/memory.h>
+#include <asm/nds32.h>
+#include <asm/errno.h>
+#include <asm/asm-offsets.h>
+#include <asm/page.h>
+
+#ifdef CONFIG_HWZOL
+ .macro push_zol
+ mfusr $r14, $LB
+ mfusr $r15, $LE
+ mfusr $r16, $LC
+ .endm
+#endif
+
+ .macro save_user_regs
+
+ smw.adm $sp, [$sp], $sp, #0x1
+ /* move $SP to the bottom of pt_regs */
+ addi $sp, $sp, -OSP_OFFSET
+
+ /* push $r0 ~ $r25 */
+ smw.bim $r0, [$sp], $r25
+ /* push $fp, $gp, $lp */
+ smw.bim $sp, [$sp], $sp, #0xe
+
+ mfsr $r12, $SP_USR
+ mfsr $r13, $IPC
+#ifdef CONFIG_HWZOL
+ push_zol
+#endif
+ movi $r17, -1
+ move $r18, $r0
+ mfsr $r19, $PSW
+ mfsr $r20, $IPSW
+ mfsr $r21, $P_IPSW
+ mfsr $r22, $P_IPC
+ mfsr $r23, $P_P0
+ mfsr $r24, $P_P1
+ smw.bim $r12, [$sp], $r24, #0
+ addi $sp, $sp, -FUCOP_CTL_OFFSET
+
+ /* Initialize kernel space $fp */
+ andi $p0, $r20, #PSW_mskPOM
+ movi $p1, #0x0
+ cmovz $fp, $p1, $p0
+
+ andi $r16, $r19, #PSW_mskINTL
+ slti $r17, $r16, #4
+ bnez $r17, 1f
+ addi $r17, $r19, #-2
+ mtsr $r17, $PSW
+ isb
+1:
+ /* If it was superuser mode, we don't need to update $r25 */
+ bnez $p0, 2f
+ la $p0, __entry_task
+ lw $r25, [$p0]
+2:
+ .endm
+
+ .text
+
+/*
+ * Exception Vector
+ */
+exception_handlers:
+ .long unhandled_exceptions !Reset/NMI
+ .long unhandled_exceptions !TLB fill
+ .long do_page_fault !PTE not present
+ .long do_dispatch_tlb_misc !TLB misc
+ .long unhandled_exceptions !TLB VLPT
+ .long unhandled_exceptions !Machine Error
+ .long do_debug_trap !Debug related
+ .long do_dispatch_general !General exception
+ .long eh_syscall !Syscall
+ .long asm_do_IRQ !IRQ
+
+common_exception_handler:
+ save_user_regs
+ mfsr $p0, $ITYPE
+ andi $p0, $p0, #ITYPE_mskVECTOR
+ srli $p0, $p0, #ITYPE_offVECTOR
+ andi $p1, $p0, #NDS32_VECTOR_mskNONEXCEPTION
+ bnez $p1, 1f
+ sethi $lp, hi20(ret_from_exception)
+ ori $lp, $lp, lo12(ret_from_exception)
+ sethi $p1, hi20(exception_handlers)
+ ori $p1, $p1, lo12(exception_handlers)
+ lw $p1, [$p1+$p0<<2]
+ move $r0, $p0
+ mfsr $r1, $EVA
+ mfsr $r2, $ITYPE
+ move $r3, $sp
+ mfsr $r4, $OIPC
+ /* enable gie if it is enabled in IPSW. */
+ mfsr $r21, $PSW
+ andi $r20, $r20, #PSW_mskGIE /* r20 is $IPSW*/
+ or $r21, $r21, $r20
+ mtsr $r21, $PSW
+ dsb
+ jr $p1
+
+ /* syscall */
+1:
+ addi $p1, $p0, #-NDS32_VECTOR_offEXCEPTION
+ bnez $p1, 2f
+ sethi $lp, hi20(ret_from_exception)
+ ori $lp, $lp, lo12(ret_from_exception)
+ sethi $p1, hi20(exception_handlers)
+ ori $p1, $p1, lo12(exception_handlers)
+ lwi $p1, [$p1+#NDS32_VECTOR_offEXCEPTION<<2]
+ jr $p1
+
+ /* interrupt */
+2:
+#ifdef CONFIG_TRACE_IRQFLAGS
+ jal arch_trace_hardirqs_off
+#endif
+ move $r0, $sp
+ sethi $lp, hi20(ret_from_intr)
+ ori $lp, $lp, lo12(ret_from_intr)
+ sethi $p0, hi20(exception_handlers)
+ ori $p0, $p0, lo12(exception_handlers)
+ lwi $p0, [$p0+#NDS32_VECTOR_offINTERRUPT<<2]
+ jr $p0
+
+ .macro EXCEPTION_VECTOR_DEBUG
+ .align 4
+ mfsr $p0, $EDM_CTL
+ andi $p0, $p0, EDM_CTL_mskV3_EDM_MODE
+ tnez $p0, SWID_RAISE_INTERRUPT_LEVEL
+ .endm
+
+ .macro EXCEPTION_VECTOR
+ .align 4
+ sethi $p0, hi20(common_exception_handler)
+ ori $p0, $p0, lo12(common_exception_handler)
+ jral.ton $p0, $p0
+ .endm
+
+ .section ".text.init", #alloc, #execinstr
+ .global exception_vector
+exception_vector:
+.rept 5
+ EXCEPTION_VECTOR
+.endr
+ EXCEPTION_VECTOR_DEBUG
+.rept 122
+ EXCEPTION_VECTOR
+.endr
+ .align 4
+ .global exception_vector_end
+exception_vector_end:
diff --git a/arch/nds32/kernel/ex-exit.S b/arch/nds32/kernel/ex-exit.S
new file mode 100644
index 0000000..fc60e60
--- /dev/null
+++ b/arch/nds32/kernel/ex-exit.S
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2005-2017 Andes Technology Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/linkage.h>
+#include <asm/unistd.h>
+#include <asm/assembler.h>
+#include <asm/nds32.h>
+#include <asm/asm-offsets.h>
+#include <asm/thread_info.h>
+#include <asm/current.h>
+
+
+
+#ifdef CONFIG_HWZOL
+ .macro pop_zol
+ mtusr $r14, $LB
+ mtusr $r15, $LE
+ mtusr $r16, $LC
+ .endm
+#endif
+
+ .macro restore_user_regs_first
+ setgie.d
+ isb
+
+ addi $sp, $sp, FUCOP_CTL_OFFSET
+
+ lmw.adm $r12, [$sp], $r24, #0x0
+ mtsr $r12, $SP_USR
+ mtsr $r13, $IPC
+#ifdef CONFIG_HWZOL
+ pop_zol
+#endif
+ mtsr $r19, $PSW
+ mtsr $r20, $IPSW
+ mtsr $r21, $P_IPSW
+ mtsr $r22, $P_IPC
+ mtsr $r23, $P_P0
+ mtsr $r24, $P_P1
+ lmw.adm $sp, [$sp], $sp, #0xe
+ .endm
+
+ .macro restore_user_regs_last
+ pop $p0
+ cmovn $sp, $p0, $p0
+
+ iret
+ nop
+
+ .endm
+
+ .macro restore_user_regs
+ restore_user_regs_first
+ lmw.adm $r0, [$sp], $r25, #0x0
+ addi $sp, $sp, OSP_OFFSET
+ restore_user_regs_last
+ .endm
+
+ .macro fast_restore_user_regs
+ restore_user_regs_first
+ lmw.adm $r1, [$sp], $r25, #0x0
+ addi $sp, $sp, OSP_OFFSET-4
+ restore_user_regs_last
+ .endm
+
+#ifdef CONFIG_PREEMPT
+ .macro preempt_stop
+ .endm
+#else
+ .macro preempt_stop
+ setgie.d
+ isb
+ .endm
+#define resume_kernel no_work_pending
+#endif
+
+ENTRY(ret_from_exception)
+ preempt_stop
+ENTRY(ret_from_intr)
+ move why, #0 ! not system call
+
+/*
+ * judge Kernel or user mode
+ *
+ */
+ lwi $p0, [$sp+(#IPSW_OFFSET)] ! Check if in nested interrupt
+ andi $p0, $p0, #PSW_mskINTL
+ bnez $p0, resume_kernel ! done with iret
+ j resume_userspace
+
+
+/*
+ * This is the fast syscall return path. We do as little as
+ * possible here, and this includes saving $r0 back into the SVC
+ * stack.
+ * fixed: tsk - $r25, why - $r8, $r7 - syscall #, $r8 - syscall table pointer
+ */
+ENTRY(ret_fast_syscall)
+ gie_disable
+ lwi $r1, [tsk+#TSK_TI_FLAGS]
+ andi $p1, $r1, #_TIF_WORK_MASK
+ bnez $p1, fast_work_pending
+ fast_restore_user_regs ! iret
+
+/*
+ * Ok, we need to do extra processing,
+ * enter the slow path returning from syscall, while pending work.
+ */
+fast_work_pending:
+ swi $r0, [$sp+(#R0_OFFSET)] ! what is different from ret_from_exception
+work_pending:
+ andi $p1, $r1, #_TIF_NEED_RESCHED
+ bnez $p1, work_resched
+
+ andi $p1, $r1, #_TIF_SIGPENDING|#_TIF_NOTIFY_RESUME
+ beqz $p1, no_work_pending
+
+ move $r0, $sp ! 'regs'
+ move $r2, why
+ gie_enable
+ bal do_notify_resume
+ beqz $r0, ret_slow_syscall
+ bgtz $r0, 1f
+ lmw.bi $r0, [sp], $r5
+ movi $r7, #__NR_restart_syscall
+1:
+ b eh_syscall_phase_2
+work_resched:
+ bal schedule ! path, return to user mode
+
+/*
+ * "slow" syscall return path.
+ * "why" tells us if this was a real syscall.
+ */
+ENTRY(resume_userspace)
+ENTRY(ret_slow_syscall)
+ gie_disable
+ lwi $p0, [$sp+(#IPSW_OFFSET)] ! Check if in nested interrupt
+ andi $p0, $p0, #PSW_mskINTL
+ bnez $p0, no_work_pending ! done with iret
+ lwi $r1, [tsk+#TSK_TI_FLAGS]
+ andi $p1, $r1, #_TIF_WORK_MASK
+ bnez $p1, work_pending ! handle work_resched, sig_pend
+
+no_work_pending:
+#ifdef CONFIG_TRACE_IRQFLAGS
+ lwi $p0, [$sp+(#IPSW_OFFSET)]
+ andi $p0, $p0, #0x1
+ la $r10, trace_hardirqs_off
+ la $r9, trace_hardirqs_on
+ cmovz $r9, $p0, $r10
+ jral $r9
+#endif
+ restore_user_regs ! return from iret
+
+
+/*
+ * preemptive kernel
+ */
+#ifdef CONFIG_PREEMPT
+resume_kernel:
+ gie_disable
+ lwi $t0, [tsk+#TSK_TI_PREEMPT]
+ bnez $t0, no_work_pending
+need_resched:
+ lwi $t0, [tsk+#TSK_TI_FLAGS]
+ andi $p1, $t0, #_TIF_NEED_RESCHED
+ beqz $p1, no_work_pending
+
+ lwi $t0, [$sp+(#IPSW_OFFSET)] ! Interrupts off?
+ andi $t0, $t0, #1
+ beqz $t0, no_work_pending
+
+ jal preempt_schedule_irq
+ b need_resched
+#endif
+
+/*
+ * This is how we return from a fork.
+ */
+ENTRY(ret_from_fork)
+ bal schedule_tail
+ beqz $r6, 1f ! r6 stores fn for kernel thread
+ move $r0, $r7 ! prepare kernel thread arg
+ jral $r6
+1:
+ lwi $r1, [tsk+#TSK_TI_FLAGS] ! check for syscall tracing
+ move why, #1
+ andi $p1, $r1, #_TIF_WORK_SYSCALL_LEAVE ! are we tracing syscalls?
+ beqz $p1, ret_slow_syscall
+ move $r0, $sp
+ bal syscall_trace_leave
+ b ret_slow_syscall
diff --git a/arch/nds32/kernel/stacktrace.c b/arch/nds32/kernel/stacktrace.c
new file mode 100644
index 0000000..78e816d
--- /dev/null
+++ b/arch/nds32/kernel/stacktrace.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2005-2017 Andes Technology Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/sched/debug.h>
+#include <linux/sched/task_stack.h>
+#include <linux/stacktrace.h>
+
+void save_stack_trace(struct stack_trace *trace)
+{
+ save_stack_trace_tsk(current, trace);
+}
+
+void save_stack_trace_tsk(struct task_struct *tsk, struct stack_trace *trace)
+{
+ unsigned long *fpn;
+ int skip = trace->skip;
+ int savesched;
+
+ if (tsk == current) {
+ __asm__ __volatile__("\tori\t%0, $fp, #0\n":"=r"(fpn));
+ savesched = 1;
+ } else {
+ fpn = (unsigned long *)thread_saved_fp(tsk);
+ savesched = 0;
+ }
+
+ while (!kstack_end(fpn) && !((unsigned long)fpn & 0x3)
+ && (fpn >= (unsigned long *)TASK_SIZE)) {
+ unsigned long lpp, fpp;
+
+ lpp = fpn[-1];
+ fpp = fpn[FP_OFFSET];
+ if (!__kernel_text_address(lpp))
+ break;
+
+ if (savesched || !in_sched_functions(lpp)) {
+ if (skip) {
+ skip--;
+ } else {
+ trace->entries[trace->nr_entries++] = lpp;
+ if (trace->nr_entries >= trace->max_entries)
+ break;
+ }
+ }
+ fpn = (unsigned long *)fpp;
+ }
+}
diff --git a/arch/nds32/kernel/traps.c b/arch/nds32/kernel/traps.c
new file mode 100644
index 0000000..30a275d
--- /dev/null
+++ b/arch/nds32/kernel/traps.c
@@ -0,0 +1,441 @@
+/*
+ * Copyright (C) 2005-2017 Andes Technology Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/personality.h>
+#include <linux/kallsyms.h>
+#include <linux/hardirq.h>
+#include <linux/kdebug.h>
+#include <linux/sched/task_stack.h>
+
+#include <asm/proc-fns.h>
+#include <asm/uaccess.h>
+#include <asm/unistd.h>
+
+#include <linux/ptrace.h>
+#include <nds32_intrinsic.h>
+
+extern void show_pte(struct mm_struct *mm, unsigned long addr);
+
+/*
+ * Dump out the contents of some memory nicely...
+ */
+void dump_mem(const char *lvl, unsigned long bottom, unsigned long top)
+{
+ unsigned long first;
+ mm_segment_t fs;
+ int i;
+
+ /*
+ * We need to switch to kernel mode so that we can use __get_user
+ * to safely read from kernel space. Note that we now dump the
+ * code first, just in case the backtrace kills us.
+ */
+ fs = get_fs();
+ set_fs(KERNEL_DS);
+
+ pr_emerg("%s(0x%08lx to 0x%08lx)\n", lvl, bottom, top);
+
+ for (first = bottom & ~31; first < top; first += 32) {
+ unsigned long p;
+ char str[sizeof(" 12345678") * 8 + 1];
+
+ memset(str, ' ', sizeof(str));
+ str[sizeof(str) - 1] = '\0';
+
+ for (p = first, i = 0; i < 8 && p < top; i++, p += 4) {
+ if (p >= bottom && p < top) {
+ unsigned long val;
+ if (__get_user(val, (unsigned long *)p) == 0)
+ sprintf(str + i * 9, " %08lx", val);
+ else
+ sprintf(str + i * 9, " ????????");
+ }
+ }
+ pr_emerg("%s%04lx:%s\n", lvl, first & 0xffff, str);
+ }
+
+ set_fs(fs);
+}
+
+EXPORT_SYMBOL(dump_mem);
+
+static void dump_instr(struct pt_regs *regs)
+{
+ unsigned long addr = instruction_pointer(regs);
+ mm_segment_t fs;
+ char str[sizeof("00000000 ") * 5 + 2 + 1], *p = str;
+ int i;
+
+ return;
+ /*
+ * We need to switch to kernel mode so that we can use __get_user
+ * to safely read from kernel space. Note that we now dump the
+ * code first, just in case the backtrace kills us.
+ */
+ fs = get_fs();
+ set_fs(KERNEL_DS);
+
+ pr_emerg("Code: ");
+ for (i = -4; i < 1; i++) {
+ unsigned int val, bad;
+
+ bad = __get_user(val, &((u32 *) addr)[i]);
+
+ if (!bad) {
+ p += sprintf(p, i == 0 ? "(%08x) " : "%08x ", val);
+ } else {
+ p += sprintf(p, "bad PC value");
+ break;
+ }
+ }
+ pr_emerg("Code: %s\n", str);
+
+ set_fs(fs);
+}
+
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+#include <linux/ftrace.h>
+static void
+get_real_ret_addr(unsigned long *addr, struct task_struct *tsk, int *graph)
+{
+ if (*addr == (unsigned long)return_to_handler) {
+ int index = tsk->curr_ret_stack;
+
+ if (tsk->ret_stack && index >= *graph) {
+ index -= *graph;
+ *addr = tsk->ret_stack[index].ret;
+ (*graph)++;
+ }
+ }
+}
+#else
+static inline void
+get_real_ret_addr(unsigned long *addr, struct task_struct *tsk, int *graph)
+{
+}
+#endif
+
+#define LOOP_TIMES (100)
+static void __dump(struct task_struct *tsk, unsigned long *base_reg)
+{
+ unsigned long ret_addr;
+ int cnt = LOOP_TIMES, graph = 0;
+ pr_emerg("Call Trace:\n");
+ if (!IS_ENABLED(CONFIG_FRAME_POINTER)) {
+ while (!kstack_end(base_reg)) {
+ ret_addr = *base_reg++;
+ if (__kernel_text_address(ret_addr)) {
+ get_real_ret_addr(&ret_addr, tsk, &graph);
+ print_ip_sym(ret_addr);
+ }
+ if (--cnt < 0)
+ break;
+ }
+ } else {
+ while (!kstack_end((void *)base_reg) &&
+ !((unsigned long)base_reg & 0x3) &&
+ ((unsigned long)base_reg >= TASK_SIZE)) {
+ unsigned long next_fp;
+#if !defined(NDS32_ABI_2)
+ ret_addr = base_reg[0];
+ next_fp = base_reg[1];
+#else
+ ret_addr = base_reg[-1];
+ next_fp = base_reg[FP_OFFSET];
+#endif
+ if (__kernel_text_address(ret_addr)) {
+ get_real_ret_addr(&ret_addr, tsk, &graph);
+ print_ip_sym(ret_addr);
+ }
+ if (--cnt < 0)
+ break;
+ base_reg = (unsigned long *)next_fp;
+ }
+ }
+ pr_emerg("\n");
+}
+
+void dump_stack(void)
+{
+ unsigned long *base_reg;
+ if (!IS_ENABLED(CONFIG_FRAME_POINTER))
+ __asm__ __volatile__("\tori\t%0, $sp, #0\n":"=r"(base_reg));
+ else
+ __asm__ __volatile__("\tori\t%0, $fp, #0\n":"=r"(base_reg));
+ __dump(NULL, base_reg);
+}
+
+EXPORT_SYMBOL(dump_stack);
+
+void show_stack(struct task_struct *tsk, unsigned long *sp)
+{
+ unsigned long *base_reg;
+
+ if (!tsk)
+ tsk = current;
+ if (!IS_ENABLED(CONFIG_FRAME_POINTER)) {
+ if (tsk != current)
+ base_reg = (unsigned long *)(tsk->thread.cpu_context.sp);
+ else
+ __asm__ __volatile__("\tori\t%0, $sp, #0\n":"=r"(base_reg));
+ } else {
+ if (tsk != current)
+ base_reg = (unsigned long *)(tsk->thread.cpu_context.fp);
+ else
+ __asm__ __volatile__("\tori\t%0, $fp, #0\n":"=r"(base_reg));
+ }
+ __dump(tsk, base_reg);
+ barrier();
+}
+
+DEFINE_SPINLOCK(die_lock);
+
+/*
+ * This function is protected against re-entrancy.
+ */
+void die(const char *str, struct pt_regs *regs, int err)
+{
+ struct task_struct *tsk = current;
+ static int die_counter;
+
+ console_verbose();
+ spin_lock_irq(&die_lock);
+ bust_spinlocks(1);
+
+ pr_emerg("Internal error: %s: %x [#%d]\n", str, err, ++die_counter);
+ print_modules();
+ pr_emerg("CPU: %i\n", smp_processor_id());
+ show_regs(regs);
+ pr_emerg("Process %s (pid: %d, stack limit = 0x%p)\n",
+ tsk->comm, tsk->pid, task_thread_info(tsk) + 1);
+
+ if (!user_mode(regs) || in_interrupt()) {
+ dump_mem("Stack: ", regs->sp,
+ THREAD_SIZE + (unsigned long)task_thread_info(tsk));
+ dump_instr(regs);
+ dump_stack();
+ }
+
+ bust_spinlocks(0);
+ spin_unlock_irq(&die_lock);
+ do_exit(SIGSEGV);
+}
+
+EXPORT_SYMBOL(die);
+
+void die_if_kernel(const char *str, struct pt_regs *regs, int err)
+{
+ if (user_mode(regs))
+ return;
+
+ die(str, regs, err);
+}
+
+int bad_syscall(int n, struct pt_regs *regs)
+{
+ siginfo_t info;
+
+ if (current->personality != PER_LINUX) {
+ send_sig(SIGSEGV, current, 1);
+ return regs->uregs[0];
+ }
+
+ info.si_signo = SIGILL;
+ info.si_errno = 0;
+ info.si_code = ILL_ILLTRP;
+ info.si_addr = (void __user *)instruction_pointer(regs) - 4;
+
+ force_sig_info(SIGILL, &info, current);
+ die_if_kernel("Oops - bad syscall", regs, n);
+ return regs->uregs[0];
+}
+
+void __pte_error(const char *file, int line, unsigned long val)
+{
+ pr_emerg("%s:%d: bad pte %08lx.\n", file, line, val);
+}
+
+void __pmd_error(const char *file, int line, unsigned long val)
+{
+ pr_emerg("%s:%d: bad pmd %08lx.\n", file, line, val);
+}
+
+void __pgd_error(const char *file, int line, unsigned long val)
+{
+ pr_emerg("%s:%d: bad pgd %08lx.\n", file, line, val);
+}
+
+extern char *exception_vector, *exception_vector_end;
+void __init trap_init(void)
+{
+ return;
+}
+
+void __init early_trap_init(void)
+{
+ unsigned long ivb = 0;
+ unsigned long base = PAGE_OFFSET;
+
+ memcpy((unsigned long *)base, (unsigned long *)&exception_vector,
+ ((unsigned long)&exception_vector_end -
+ (unsigned long)&exception_vector));
+ ivb = __nds32__mfsr(NDS32_SR_IVB);
+ /* Check platform support. */
+ if (((ivb & IVB_mskNIVIC) >> IVB_offNIVIC) < 2)
+ panic
+ ("IVIC mode is not allowed on the platform with interrupt controller\n");
+ __nds32__mtsr((ivb & ~IVB_mskESZ) | (IVB_valESZ16 << IVB_offESZ) |
+ IVB_BASE, NDS32_SR_IVB);
+ __nds32__mtsr(INT_MASK_INITAIAL_VAL, NDS32_SR_INT_MASK);
+
+ /*
+ * 0x800 = 128 vectors * 16byte.
+ * It should be enough to flush a page.
+ */
+ cpu_cache_wbinval_page(base, true);
+}
+
+void do_debug_trap(unsigned long entry, unsigned long addr,
+ unsigned long type, struct pt_regs *regs)
+{
+ if (notify_die(DIE_OOPS, "Oops", regs, addr, type, SIGTRAP)
+ == NOTIFY_STOP)
+ return;
+
+ if (user_mode(regs)) {
+ /* trap_signal */
+ send_sigtrap(current, regs, 0, TRAP_BRKPT);
+ } else {
+ /* kernel_trap */
+ if (!fixup_exception(regs))
+ die("unexpected kernel_trap", regs, 0);
+ }
+}
+
+void unhandled_interruption(struct pt_regs *regs)
+{
+ siginfo_t si;
+ pr_emerg("unhandled_interruption\n");
+ show_regs(regs);
+ if (!user_mode(regs))
+ do_exit(SIGKILL);
+ si.si_signo = SIGKILL;
+ si.si_errno = 0;
+ force_sig_info(SIGKILL, &si, current);
+}
+
+void unhandled_exceptions(unsigned long entry, unsigned long addr,
+ unsigned long type, struct pt_regs *regs)
+{
+ siginfo_t si;
+ pr_emerg("Unhandled Exception: entry: %lx addr:%lx itype:%lx\n", entry,
+ addr, type);
+ show_regs(regs);
+ if (!user_mode(regs))
+ do_exit(SIGKILL);
+ si.si_signo = SIGKILL;
+ si.si_errno = 0;
+ si.si_addr = (void *)addr;
+ force_sig_info(SIGKILL, &si, current);
+}
+
+extern int do_page_fault(unsigned long entry, unsigned long addr,
+ unsigned int error_code, struct pt_regs *regs);
+
+/*
+ * 2:DEF dispatch for TLB MISC exception handler
+*/
+
+void do_dispatch_tlb_misc(unsigned long entry, unsigned long addr,
+ unsigned long type, struct pt_regs *regs)
+{
+ type = type & (ITYPE_mskINST | ITYPE_mskETYPE);
+ if ((type & ITYPE_mskETYPE) < 5) {
+ /* Permission exceptions */
+ do_page_fault(entry, addr, type, regs);
+ } else
+ unhandled_exceptions(entry, addr, type, regs);
+}
+
+void do_revinsn(struct pt_regs *regs)
+{
+ siginfo_t si;
+ pr_emerg("Reserved Instruction\n");
+ show_regs(regs);
+ if (!user_mode(regs))
+ do_exit(SIGILL);
+ si.si_signo = SIGILL;
+ si.si_errno = 0;
+ force_sig_info(SIGILL, &si, current);
+}
+
+#ifdef CONFIG_ALIGNMENT_TRAP
+extern int unalign_access_mode;
+extern int do_unaligned_access(unsigned long entry, unsigned long addr,
+ unsigned long type, struct pt_regs *regs);
+#endif
+void do_dispatch_general(unsigned long entry, unsigned long addr,
+ unsigned long itype, struct pt_regs *regs,
+ unsigned long oipc)
+{
+ unsigned int swid = itype >> ITYPE_offSWID;
+ unsigned long type = itype & (ITYPE_mskINST | ITYPE_mskETYPE);
+ if (type == ETYPE_ALIGNMENT_CHECK) {
+#ifdef CONFIG_ALIGNMENT_TRAP
+ /* Alignment check */
+ if (unalign_access_mode & 0x3) {
+ int ret;
+ ret = do_unaligned_access(entry, addr, type, regs);
+
+ if (ret == 0)
+ return;
+
+ if (ret == -EFAULT)
+ pr_emerg
+ ("Unhandled unaligned access exception\n");
+ }
+#endif
+ do_page_fault(entry, addr, type, regs);
+ } else if (type == ETYPE_RESERVED_INSTRUCTION) {
+ /* Reserved instruction */
+ do_revinsn(regs);
+ } else if (type == ETYPE_TRAP && swid == SWID_RAISE_INTERRUPT_LEVEL) {
+ /* trap, used on v3 EDM target debugging workaround */
+ /*
+ * DIPC(OIPC) is passed as parameter before
+ * interrupt is enabled, so the DIPC will not be corrupted
+ * even though interrupts are coming in
+ */
+ /*
+ * 1. update ipc
+ * 2. update pt_regs ipc with oipc
+ * 3. update pt_regs ipsw (clear DEX)
+ */
+ __asm__ volatile ("mtsr %0, $IPC\n\t"::"r" (oipc));
+ regs->ipc = oipc;
+ if (regs->pipsw & PSW_mskDEX) {
+ pr_emerg
+ ("Nested Debug exception is possibly happened\n");
+ pr_emerg("ipc:%08x pipc:%08x\n",
+ (unsigned int)regs->ipc,
+ (unsigned int)regs->pipc);
+ }
+ do_debug_trap(entry, addr, itype, regs);
+ regs->ipsw &= ~PSW_mskDEX;
+ } else
+ unhandled_exceptions(entry, addr, type, regs);
+}
diff --git a/arch/nds32/mm/alignment.c b/arch/nds32/mm/alignment.c
new file mode 100644
index 0000000..19aba59
--- /dev/null
+++ b/arch/nds32/mm/alignment.c
@@ -0,0 +1,622 @@
+/*
+ * Copyright (C) 2005-2017 Andes Technology Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/proc_fs.h>
+#include <linux/uaccess.h>
+#include <linux/sysctl.h>
+#include <asm/unaligned.h>
+
+#define DEBUG(enable, tagged, ...) \
+ do{ \
+ if (enable) { \
+ if (tagged) \
+ pr_warn("[ %30s() ] ", __func__); \
+ pr_warn(__VA_ARGS__); \
+ } \
+ } while (0)
+
+#define RT(inst) (((inst) >> 20) & 0x1FUL)
+#define RA(inst) (((inst) >> 15) & 0x1FUL)
+#define RB(inst) (((inst) >> 10) & 0x1FUL)
+#define SV(inst) (((inst) >> 8) & 0x3UL)
+#define IMM(inst) (((inst) >> 0) & 0x3FFFUL)
+
+#define RA3(inst) (((inst) >> 3) & 0x7UL)
+#define RT3(inst) (((inst) >> 6) & 0x7UL)
+#define IMM3U(inst) (((inst) >> 0) & 0x7UL)
+
+#define RA5(inst) (((inst) >> 0) & 0x1FUL)
+#define RT4(inst) (((inst) >> 5) & 0xFUL)
+
+#define __get8_data(val,addr,err) \
+ __asm__( \
+ "1: lbi.bi %1, [%2], #1\n" \
+ "2:\n" \
+ " .pushsection .text.fixup,\"ax\"\n" \
+ " .align 2\n" \
+ "3: movi %0, #1\n" \
+ " j 2b\n" \
+ " .popsection\n" \
+ " .pushsection __ex_table,\"a\"\n" \
+ " .align 3\n" \
+ " .long 1b, 3b\n" \
+ " .popsection\n" \
+ : "=r" (err), "=&r" (val), "=r" (addr) \
+ : "0" (err), "2" (addr))
+
+#define get16_data(addr, val_ptr) \
+ do { \
+ unsigned int err = 0, v, a = addr; \
+ __get8_data(v,a,err); \
+ *val_ptr = v << 0; \
+ __get8_data(v,a,err); \
+ *val_ptr |= v << 8; \
+ if (err) \
+ goto fault; \
+ *val_ptr = le16_to_cpu(*val_ptr); \
+ } while(0)
+
+#define get32_data(addr, val_ptr) \
+ do { \
+ unsigned int err = 0, v, a = addr; \
+ __get8_data(v,a,err); \
+ *val_ptr = v << 0; \
+ __get8_data(v,a,err); \
+ *val_ptr |= v << 8; \
+ __get8_data(v,a,err); \
+ *val_ptr |= v << 16; \
+ __get8_data(v,a,err); \
+ *val_ptr |= v << 24; \
+ if (err) \
+ goto fault; \
+ *val_ptr = le32_to_cpu(*val_ptr); \
+ } while(0)
+
+#define get_data(addr, val_ptr, len) \
+ if (len == 2) \
+ get16_data(addr, val_ptr); \
+ else \
+ get32_data(addr, val_ptr);
+
+
+#define set16_data(addr, val) \
+ do { \
+ unsigned int err = 0, *ptr = addr ; \
+ val = le32_to_cpu(val); \
+ __asm__( \
+ "1: sbi.bi %2, [%1], #1\n" \
+ " srli %2, %2, #8\n" \
+ "2: sbi %2, [%1]\n" \
+ "3:\n" \
+ " .pushsection .text.fixup,\"ax\"\n" \
+ " .align 2\n" \
+ "4: movi %0, #1\n" \
+ " j 3b\n" \
+ " .popsection\n" \
+ " .pushsection __ex_table,\"a\"\n" \
+ " .align 3\n" \
+ " .long 1b, 4b\n" \
+ " .long 2b, 4b\n" \
+ " .popsection\n" \
+ : "=r" (err), "+r" (ptr), "+r" (val) \
+ : "0" (err) \
+ ); \
+ if (err) \
+ goto fault; \
+ } while(0)
+
+#define set32_data(addr, val) \
+ do { \
+ unsigned int err = 0, *ptr = addr ; \
+ val = le32_to_cpu(val); \
+ __asm__( \
+ "1: sbi.bi %2, [%1], #1\n" \
+ " srli %2, %2, #8\n" \
+ "2: sbi.bi %2, [%1], #1\n" \
+ " srli %2, %2, #8\n" \
+ "3: sbi.bi %2, [%1], #1\n" \
+ " srli %2, %2, #8\n" \
+ "4: sbi %2, [%1]\n" \
+ "5:\n" \
+ " .pushsection .text.fixup,\"ax\"\n" \
+ " .align 2\n" \
+ "6: movi %0, #1\n" \
+ " j 5b\n" \
+ " .popsection\n" \
+ " .pushsection __ex_table,\"a\"\n" \
+ " .align 3\n" \
+ " .long 1b, 6b\n" \
+ " .long 2b, 6b\n" \
+ " .long 3b, 6b\n" \
+ " .long 4b, 6b\n" \
+ " .popsection\n" \
+ : "=r" (err), "+r" (ptr), "+r" (val) \
+ : "0" (err) \
+ ); \
+ if (err) \
+ goto fault; \
+ } while(0)
+#define set_data(addr, val, len) \
+ if (len == 2) \
+ set16_data(addr, val); \
+ else \
+ set32_data(addr, val);
+extern pte_t va_present(struct mm_struct *mm, unsigned long addr);
+extern pte_t va_kernel_present(unsigned long addr);
+extern int va_readable(struct pt_regs *regs, unsigned long addr);
+extern int va_writable(struct pt_regs *regs, unsigned long addr);
+
+int unalign_access_mode = 0, unalign_access_debug = 0;
+
+static inline unsigned long *idx_to_addr(struct pt_regs *regs, int idx)
+{
+ /* this should be consistent with ptrace.h */
+ if (idx >= 0 && idx <= 25) /* R0-R25 */
+ return &regs->uregs[0] + idx;
+ else if (idx >= 28 && idx <= 30) /* FP, GP, LP */
+ return &regs->fp + (idx - 28);
+ else if (idx == 31) /* SP */
+ return &regs->sp;
+ else
+ return NULL; /* cause a segfault */
+}
+
+static inline unsigned long get_inst(unsigned long addr)
+{
+ return be32_to_cpu(get_unaligned((u32 *) addr));
+}
+
+
+static inline unsigned long sign_extend(unsigned long val, int len)
+{
+ unsigned long ret = 0;
+ unsigned char *s, *t;
+ int i = 0;
+
+ val = cpu_to_le32(val);
+
+ s = (void *)&val;
+ t = (void *)&ret;
+
+ while (i++ < len)
+ *t++ = *s++;
+
+ if (((*(t - 1)) & 0x80) && (i < 4)) {
+
+ while (i++ <= 4)
+ *t++ = 0xff;
+ }
+
+ return le32_to_cpu(ret);
+}
+
+static inline int do_16(unsigned long inst, struct pt_regs *regs)
+{
+ int imm, regular, load, len, addr_mode, idx_mode;
+ unsigned long unaligned_addr, target_val, source_idx, target_idx,
+ shift = 0;
+ switch ((inst >> 9) & 0x3F) {
+
+ case 0x12: /* LHI333 */
+ imm = 1;
+ regular = 1;
+ load = 1;
+ len = 2;
+ addr_mode = 3;
+ idx_mode = 3;
+ break;
+ case 0x10: /* LWI333 */
+ imm = 1;
+ regular = 1;
+ load = 1;
+ len = 4;
+ addr_mode = 3;
+ idx_mode = 3;
+ break;
+ case 0x11: /* LWI333.bi */
+ imm = 1;
+ regular = 0;
+ load = 1;
+ len = 4;
+ addr_mode = 3;
+ idx_mode = 3;
+ break;
+ case 0x1A: /* LWI450 */
+ imm = 0;
+ regular = 1;
+ load = 1;
+ len = 4;
+ addr_mode = 5;
+ idx_mode = 4;
+ break;
+ case 0x16: /* SHI333 */
+ imm = 1;
+ regular = 1;
+ load = 0;
+ len = 2;
+ addr_mode = 3;
+ idx_mode = 3;
+ break;
+ case 0x14: /* SWI333 */
+ imm = 1;
+ regular = 1;
+ load = 0;
+ len = 4;
+ addr_mode = 3;
+ idx_mode = 3;
+ break;
+ case 0x15: /* SWI333.bi */
+ imm = 1;
+ regular = 0;
+ load = 0;
+ len = 4;
+ addr_mode = 3;
+ idx_mode = 3;
+ break;
+ case 0x1B: /* SWI450 */
+ imm = 0;
+ regular = 1;
+ load = 0;
+ len = 4;
+ addr_mode = 5;
+ idx_mode = 4;
+ break;
+
+ default:
+ return -EFAULT;
+ }
+
+ if (addr_mode == 3) {
+ unaligned_addr = *idx_to_addr(regs, RA3(inst));
+ source_idx = RA3(inst);
+ } else {
+ unaligned_addr = *idx_to_addr(regs, RA5(inst));
+ source_idx = RA5(inst);
+ }
+
+ if (idx_mode == 3)
+ target_idx = RT3(inst);
+ else
+ target_idx = RT4(inst);
+
+ if (imm)
+ shift = IMM3U(inst) * len;
+
+ if (regular)
+ unaligned_addr += shift;
+
+ if (load) {
+ if (!access_ok(VERIFY_READ, (void *)unaligned_addr, len))
+ return -EACCES;
+
+ get_data(unaligned_addr,&target_val,len);
+ *idx_to_addr(regs, target_idx) = target_val;
+ } else {
+ if (!access_ok(VERIFY_WRITE, (void *)unaligned_addr, len))
+ return -EACCES;
+ target_val = *idx_to_addr(regs, target_idx);
+ set_data((void *)unaligned_addr, target_val, len);
+ }
+
+ if(!regular)
+ *idx_to_addr(regs, source_idx) = unaligned_addr + shift;
+ regs->ipc += 2;
+
+ return 0;
+fault:
+ return -EACCES;
+}
+
+static inline int do_32(unsigned long inst, struct pt_regs *regs)
+{
+ int imm, regular, load, len, sign_ext;
+ unsigned long unaligned_addr, target_val, shift;
+
+ unaligned_addr = *idx_to_addr(regs, RA(inst));
+
+ switch ((inst >> 25) << 1) {
+
+ case 0x02: /* LHI */
+ imm = 1;
+ regular = 1;
+ load = 1;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x0A: /* LHI.bi */
+ imm = 1;
+ regular = 0;
+ load = 1;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x22: /* LHSI */
+ imm = 1;
+ regular = 1;
+ load = 1;
+ len = 2;
+ sign_ext = 1;
+ break;
+ case 0x2A: /* LHSI.bi */
+ imm = 1;
+ regular = 0;
+ load = 1;
+ len = 2;
+ sign_ext = 1;
+ break;
+ case 0x04: /* LWI */
+ imm = 1;
+ regular = 1;
+ load = 1;
+ len = 4;
+ sign_ext = 0;
+ break;
+ case 0x0C: /* LWI.bi */
+ imm = 1;
+ regular = 0;
+ load = 1;
+ len = 4;
+ sign_ext = 0;
+ break;
+ case 0x12: /* SHI */
+ imm = 1;
+ regular = 1;
+ load = 0;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x1A: /* SHI.bi */
+ imm = 1;
+ regular = 0;
+ load = 0;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x14: /* SWI */
+ imm = 1;
+ regular = 1;
+ load = 0;
+ len = 4;
+ sign_ext = 0;
+ break;
+ case 0x1C: /* SWI.bi */
+ imm = 1;
+ regular = 0;
+ load = 0;
+ len = 4;
+ sign_ext = 0;
+ break;
+
+ default:
+ switch (inst & 0xff) {
+
+ case 0x01: /* LH */
+ imm = 0;
+ regular = 1;
+ load = 1;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x05: /* LH.bi */
+ imm = 0;
+ regular = 0;
+ load = 1;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x11: /* LHS */
+ imm = 0;
+ regular = 1;
+ load = 1;
+ len = 2;
+ sign_ext = 1;
+ break;
+ case 0x15: /* LHS.bi */
+ imm = 0;
+ regular = 0;
+ load = 1;
+ len = 2;
+ sign_ext = 1;
+ break;
+ case 0x02: /* LW */
+ imm = 0;
+ regular = 1;
+ load = 1;
+ len = 4;
+ sign_ext = 0;
+ break;
+ case 0x06: /* LW.bi */
+ imm = 0;
+ regular = 0;
+ load = 1;
+ len = 4;
+ sign_ext = 0;
+ break;
+ case 0x09: /* SH */
+ imm = 0;
+ regular = 1;
+ load = 0;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x0D: /* SH.bi */
+ imm = 0;
+ regular = 0;
+ load = 0;
+ len = 2;
+ sign_ext = 0;
+ break;
+ case 0x0A: /* SW */
+ imm = 0;
+ regular = 1;
+ load = 0;
+ len = 4;
+ sign_ext = 0;
+ break;
+ case 0x0E: /* SW.bi */
+ imm = 0;
+ regular = 0;
+ load = 0;
+ len = 4;
+ sign_ext = 0;
+ break;
+
+ default:
+ return -EFAULT;
+ }
+ }
+
+ if (imm)
+ shift = IMM(inst) * len;
+ else
+ shift = *idx_to_addr(regs, RB(inst)) << SV(inst);
+
+
+ if (regular)
+ unaligned_addr += shift;
+
+ if (load) {
+
+ if (!access_ok(VERIFY_READ, (void *)unaligned_addr, len))
+ return -EACCES;
+
+ get_data(unaligned_addr,&target_val,len);
+
+ if (sign_ext)
+ *idx_to_addr(regs, RT(inst)) =
+ sign_extend(target_val, len);
+ else
+ *idx_to_addr(regs, RT(inst)) = target_val;
+ } else {
+
+ if (!access_ok(VERIFY_WRITE, (void *)unaligned_addr, len))
+ return -EACCES;
+
+ target_val = *idx_to_addr(regs, RT(inst));
+ set_data((void *)unaligned_addr, target_val, len);
+ }
+
+ if(!regular)
+ *idx_to_addr(regs, RA(inst)) = unaligned_addr + shift;
+
+ regs->ipc += 4;
+
+ return 0;
+fault:
+ return -EACCES;
+}
+
+int do_unaligned_access(unsigned long entry, unsigned long addr,
+ unsigned long type, struct pt_regs *regs)
+{
+ unsigned long inst;
+ int ret = -EFAULT;
+
+ inst = get_inst(regs->ipc);
+
+ DEBUG((unalign_access_debug > 0), 1,
+ "Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n"
+ "unalign_access_mode: %x\n"
+ , addr,regs->ipc, inst, unalign_access_mode);
+
+ if ((user_mode(regs) && (unalign_access_mode & 0x01))
+ || (!user_mode(regs) && (unalign_access_mode & 0x02))) {
+
+ mm_segment_t seg = get_fs();
+
+ user_mode(regs)?set_fs(USER_DS):set_fs(KERNEL_DS);
+
+ if (inst & 0x80000000)
+ {
+ ret = do_16((inst >> 16) & 0xffff, regs);
+ }
+ else
+ {
+ ret = do_32(inst, regs);
+ }
+ set_fs(seg);
+ }
+
+ return ret;
+}
+
+#ifdef CONFIG_PROC_FS
+
+static
+int unalign_access_policy(struct ctl_table *ctl, int write,
+ void __user * buffer, size_t * lenp, loff_t * ppos)
+{
+ int val = *(int *)(ctl->data);
+ loff_t pos = *ppos;
+ struct ctl_table lctl;
+ int ret;
+
+ lctl = *ctl;
+ lctl.data = &val;
+ ret = proc_dointvec(&lctl, write, buffer, lenp, ppos);
+
+ if (ret)
+ *ppos = pos;
+ else {
+ *(int *)ctl->data = val;
+ if (val > 3)
+ return -EINVAL;
+ }
+
+ return ret;
+}
+
+static struct ctl_table alignment_tbl[3] = {
+ {
+ .procname = "enable_mode",
+ .data = &unalign_access_mode,
+ .maxlen = sizeof(unalign_access_mode),
+ .mode = 0666,
+ .proc_handler = unalign_access_policy,
+ },
+ {
+ .procname = "debug",
+ .data = &unalign_access_debug,
+ .maxlen = sizeof(unalign_access_mode),
+ .mode = 0644,
+ .proc_handler = &proc_dointvec
+ },
+ {}
+};
+
+static struct ctl_table nds32_sysctl_table[2] = {
+ {
+ .procname = "unaligned_acess",
+ .mode = 0555,
+ .child = alignment_tbl},
+ {}
+};
+static struct ctl_path nds32_path[2] = {
+ {.procname = "nds32" },
+ {}
+};
+
+/*
+ * Initialize nds32 alignment-correction interface
+ */
+static int __init nds32_sysctl_init(void)
+{
+ register_sysctl_paths(nds32_path, nds32_sysctl_table);
+ return 0;
+}
+
+__initcall(nds32_sysctl_init);
+#endif /* CONFIG_PROC_FS */
--
1.7.9.5