[linux-2.6-block.git] / arch / nds32 / mm / alignment.c

// SPDX-License-Identifier: GPL-2.0
// Copyright (C) 2005-2017 Andes Technology Corporation

#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) & 0x7FFFUL)

#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 GET_IMMSVAL(imm_value) \
	(((imm_value >> 14) & 0x1) ? (imm_value - 0x8000) : imm_value)

#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);
#define NDS32_16BIT_INSTRUCTION	0x80000000

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((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((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 = GET_IMMSVAL(IMM(inst)) * len;
	else
		shift = *idx_to_addr(regs, RB(inst)) << SV(inst);

	if (regular)
		unaligned_addr += shift;

	if (load) {

		if (!access_ok((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((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 addr, struct pt_regs *regs)
{
	unsigned long inst;
	int ret = -EFAULT;
	mm_segment_t seg = get_fs();

	inst = get_inst(regs->ipc);

	DEBUG((unalign_access_debug > 0), 1,
	      "Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n", addr,
	      regs->ipc, inst);

	set_fs(USER_DS);

	if (inst & NDS32_16BIT_INSTRUCTION)
		ret = do_16((inst >> 16) & 0xffff, regs);
	else
		ret = do_32(inst, regs);
	set_fs(seg);

	return ret;
}

#ifdef CONFIG_PROC_FS

static struct ctl_table alignment_tbl[3] = {
	{
	 .procname = "enable",
	 .data = &unalign_access_mode,
	 .maxlen = sizeof(unalign_access_mode),
	 .mode = 0666,
	 .proc_handler = &proc_dointvec
	}
	,
	{
	 .procname = "debug_info",
	 .data = &unalign_access_debug,
	 .maxlen = sizeof(unalign_access_debug),
	 .mode = 0644,
	 .proc_handler = &proc_dointvec
	}
	,
	{}
};

static struct ctl_table nds32_sysctl_table[2] = {
	{
	 .procname = "unaligned_access",
	 .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 */
Commit	Line	Data
2923f5ea GH	1	// SPDX-License-Identifier: GPL-2.0
	2	// Copyright (C) 2005-2017 Andes Technology Corporation
	3
	4	#include <linux/proc_fs.h>
	5	#include <linux/uaccess.h>
	6	#include <linux/sysctl.h>
	7	#include <asm/unaligned.h>
	8
	9	#define DEBUG(enable, tagged, ...) \
	10	do{ \
	11	if (enable) { \
	12	if (tagged) \
	13	pr_warn("[ %30s() ] ", __func__); \
	14	pr_warn(__VA_ARGS__); \
	15	} \
	16	} while (0)
	17
	18	#define RT(inst) (((inst) >> 20) & 0x1FUL)
	19	#define RA(inst) (((inst) >> 15) & 0x1FUL)
	20	#define RB(inst) (((inst) >> 10) & 0x1FUL)
	21	#define SV(inst) (((inst) >> 8) & 0x3UL)
1613de8a	22	#define IMM(inst) (((inst) >> 0) & 0x7FFFUL)
2923f5ea GH	23
	24	#define RA3(inst) (((inst) >> 3) & 0x7UL)
	25	#define RT3(inst) (((inst) >> 6) & 0x7UL)
	26	#define IMM3U(inst) (((inst) >> 0) & 0x7UL)
	27
	28	#define RA5(inst) (((inst) >> 0) & 0x1FUL)
	29	#define RT4(inst) (((inst) >> 5) & 0xFUL)
	30
1613de8a N	31	#define GET_IMMSVAL(imm_value) \
	32	(((imm_value >> 14) & 0x1) ? (imm_value - 0x8000) : imm_value)
	33
2923f5ea GH	34	#define __get8_data(val,addr,err) \
	35	__asm__( \
	36	"1: lbi.bi %1, [%2], #1\n" \
	37	"2:\n" \
	38	" .pushsection .text.fixup,\"ax\"\n" \
	39	" .align 2\n" \
	40	"3: movi %0, #1\n" \
	41	" j 2b\n" \
	42	" .popsection\n" \
	43	" .pushsection __ex_table,\"a\"\n" \
	44	" .align 3\n" \
	45	" .long 1b, 3b\n" \
	46	" .popsection\n" \
	47	: "=r" (err), "=&r" (val), "=r" (addr) \
	48	: "0" (err), "2" (addr))
	49
	50	#define get16_data(addr, val_ptr) \
	51	do { \
	52	unsigned int err = 0, v, a = addr; \
	53	__get8_data(v,a,err); \
	54	*val_ptr = v << 0; \
	55	__get8_data(v,a,err); \
	56	*val_ptr \|= v << 8; \
	57	if (err) \
	58	goto fault; \
	59	val_ptr = le16_to_cpu(val_ptr); \
	60	} while(0)
	61
	62	#define get32_data(addr, val_ptr) \
	63	do { \
	64	unsigned int err = 0, v, a = addr; \
	65	__get8_data(v,a,err); \
	66	*val_ptr = v << 0; \
	67	__get8_data(v,a,err); \
	68	*val_ptr \|= v << 8; \
	69	__get8_data(v,a,err); \
	70	*val_ptr \|= v << 16; \
	71	__get8_data(v,a,err); \
	72	*val_ptr \|= v << 24; \
	73	if (err) \
	74	goto fault; \
	75	val_ptr = le32_to_cpu(val_ptr); \
	76	} while(0)
	77
	78	#define get_data(addr, val_ptr, len) \
	79	if (len == 2) \
	80	get16_data(addr, val_ptr); \
	81	else \
	82	get32_data(addr, val_ptr);
	83
	84	#define set16_data(addr, val) \
	85	do { \
	86	unsigned int err = 0, *ptr = addr ; \
	87	val = le32_to_cpu(val); \
	88	__asm__( \
	89	"1: sbi.bi %2, [%1], #1\n" \
	90	" srli %2, %2, #8\n" \
	91	"2: sbi %2, [%1]\n" \
	92	"3:\n" \
	93	" .pushsection .text.fixup,\"ax\"\n" \
	94	" .align 2\n" \
	95	"4: movi %0, #1\n" \
	96	" j 3b\n" \
	97	" .popsection\n" \
98	" .pushsection __ex_table,\"a\"\n" \
99	" .align 3\n" \
100	" .long 1b, 4b\n" \
101	" .long 2b, 4b\n" \
102	" .popsection\n" \
103	: "=r" (err), "+r" (ptr), "+r" (val) \
104	: "0" (err) \
105	); \
106	if (err) \
107	goto fault; \
108	} while(0)
109
110	#define set32_data(addr, val) \
111	do { \
112	unsigned int err = 0, *ptr = addr ; \
113	val = le32_to_cpu(val); \
114	__asm__( \
115	"1: sbi.bi %2, [%1], #1\n" \
116	" srli %2, %2, #8\n" \
117	"2: sbi.bi %2, [%1], #1\n" \
118	" srli %2, %2, #8\n" \
119	"3: sbi.bi %2, [%1], #1\n" \
120	" srli %2, %2, #8\n" \
121	"4: sbi %2, [%1]\n" \
122	"5:\n" \
123	" .pushsection .text.fixup,\"ax\"\n" \
124	" .align 2\n" \
125	"6: movi %0, #1\n" \
126	" j 5b\n" \
127	" .popsection\n" \
128	" .pushsection __ex_table,\"a\"\n" \
129	" .align 3\n" \
130	" .long 1b, 6b\n" \
131	" .long 2b, 6b\n" \
132	" .long 3b, 6b\n" \
133	" .long 4b, 6b\n" \
134	" .popsection\n" \
135	: "=r" (err), "+r" (ptr), "+r" (val) \
136	: "0" (err) \
137	); \
138	if (err) \
139	goto fault; \
140	} while(0)
141	#define set_data(addr, val, len) \
142	if (len == 2) \
143	set16_data(addr, val); \
144	else \
145	set32_data(addr, val);
146	#define NDS32_16BIT_INSTRUCTION 0x80000000
147
148	extern pte_t va_present(struct mm_struct *mm, unsigned long addr);
149	extern pte_t va_kernel_present(unsigned long addr);
150	extern int va_readable(struct pt_regs *regs, unsigned long addr);
151	extern int va_writable(struct pt_regs *regs, unsigned long addr);
152
153	int unalign_access_mode = 0, unalign_access_debug = 0;
154
155	static inline unsigned long idx_to_addr(struct pt_regs regs, int idx)
156	{
157	/* this should be consistent with ptrace.h */
158	if (idx >= 0 && idx <= 25) /* R0-R25 */
159	return &regs->uregs[0] + idx;
160	else if (idx >= 28 && idx <= 30) /* FP, GP, LP */
161	return &regs->fp + (idx - 28);
162	else if (idx == 31) /* SP */
163	return &regs->sp;
164	else
165	return NULL; /* cause a segfault */
166	}
167
168	static inline unsigned long get_inst(unsigned long addr)
169	{
170	return be32_to_cpu(get_unaligned((u32 *) addr));
171	}
172
173	static inline unsigned long sign_extend(unsigned long val, int len)
174	{
175	unsigned long ret = 0;
176	unsigned char s, t;
177	int i = 0;
178
179	val = cpu_to_le32(val);
180
181	s = (void *)&val;
182	t = (void *)&ret;
183
184	while (i++ < len)
185	t++ = s++;
186
187	if (((*(t - 1)) & 0x80) && (i < 4)) {
188
189	while (i++ <= 4)
190	*t++ = 0xff;
191	}
192
193	return le32_to_cpu(ret);
194	}
195
196	static inline int do_16(unsigned long inst, struct pt_regs *regs)
197	{
198	int imm, regular, load, len, addr_mode, idx_mode;
199	unsigned long unaligned_addr, target_val, source_idx, target_idx,
200	shift = 0;
201	switch ((inst >> 9) & 0x3F) {
202
203	case 0x12: /* LHI333 */
204	imm = 1;
205	regular = 1;
206	load = 1;
207	len = 2;
208	addr_mode = 3;
209	idx_mode = 3;
210	break;
211	case 0x10: /* LWI333 */
212	imm = 1;
213	regular = 1;
214	load = 1;
215	len = 4;
216	addr_mode = 3;
217	idx_mode = 3;
218	break;
219	case 0x11: /* LWI333.bi */
220	imm = 1;
221	regular = 0;
222	load = 1;
223	len = 4;
224	addr_mode = 3;
225	idx_mode = 3;
226	break;
227	case 0x1A: /* LWI450 */
228	imm = 0;
229	regular = 1;
230	load = 1;
231	len = 4;
232	addr_mode = 5;
233	idx_mode = 4;
234	break;
235	case 0x16: /* SHI333 */
236	imm = 1;
237	regular = 1;
238	load = 0;
239	len = 2;
240	addr_mode = 3;
241	idx_mode = 3;
242	break;
243	case 0x14: /* SWI333 */
244	imm = 1;
245	regular = 1;
246	load = 0;
247	len = 4;
248	addr_mode = 3;
249	idx_mode = 3;
250	break;
251	case 0x15: /* SWI333.bi */
252	imm = 1;
253	regular = 0;
254	load = 0;
255	len = 4;
256	addr_mode = 3;
257	idx_mode = 3;
258	break;
259	case 0x1B: /* SWI450 */
260	imm = 0;
261	regular = 1;
262	load = 0;
263	len = 4;
264	addr_mode = 5;
265	idx_mode = 4;
266	break;
267
268	default:
269	return -EFAULT;
270	}
271
272	if (addr_mode == 3) {
273	unaligned_addr = *idx_to_addr(regs, RA3(inst));
274	source_idx = RA3(inst);
275	} else {
276	unaligned_addr = *idx_to_addr(regs, RA5(inst));
277	source_idx = RA5(inst);
278	}
279
280	if (idx_mode == 3)
281	target_idx = RT3(inst);
282	else
283	target_idx = RT4(inst);
284
285	if (imm)
286	shift = IMM3U(inst) * len;
287
288	if (regular)
289	unaligned_addr += shift;
290
291	if (load) {
96d4f267	292	if (!access_ok((void *)unaligned_addr, len))
2923f5ea GH	293	return -EACCES;
	294
	295	get_data(unaligned_addr, &target_val, len);
	296	*idx_to_addr(regs, target_idx) = target_val;
	297	} else {
96d4f267	298	if (!access_ok((void *)unaligned_addr, len))
2923f5ea GH	299	return -EACCES;
	300	target_val = *idx_to_addr(regs, target_idx);
	301	set_data((void *)unaligned_addr, target_val, len);
	302	}
	303
	304	if (!regular)
	305	*idx_to_addr(regs, source_idx) = unaligned_addr + shift;
	306	regs->ipc += 2;
	307
	308	return 0;
	309	fault:
	310	return -EACCES;
	311	}
	312
	313	static inline int do_32(unsigned long inst, struct pt_regs *regs)
	314	{
	315	int imm, regular, load, len, sign_ext;
	316	unsigned long unaligned_addr, target_val, shift;
	317
	318	unaligned_addr = *idx_to_addr(regs, RA(inst));
	319
	320	switch ((inst >> 25) << 1) {
	321
	322	case 0x02: /* LHI */
	323	imm = 1;
	324	regular = 1;
	325	load = 1;
	326	len = 2;
	327	sign_ext = 0;
	328	break;
	329	case 0x0A: /* LHI.bi */
	330	imm = 1;
	331	regular = 0;
	332	load = 1;
	333	len = 2;
	334	sign_ext = 0;
	335	break;
	336	case 0x22: /* LHSI */
	337	imm = 1;
	338	regular = 1;
	339	load = 1;
	340	len = 2;
	341	sign_ext = 1;
	342	break;
	343	case 0x2A: /* LHSI.bi */
	344	imm = 1;
	345	regular = 0;
	346	load = 1;
	347	len = 2;
	348	sign_ext = 1;
	349	break;
	350	case 0x04: /* LWI */
	351	imm = 1;
	352	regular = 1;
	353	load = 1;
	354	len = 4;
	355	sign_ext = 0;
	356	break;
	357	case 0x0C: /* LWI.bi */
	358	imm = 1;
	359	regular = 0;
	360	load = 1;
	361	len = 4;
	362	sign_ext = 0;
363	break;
364	case 0x12: /* SHI */
365	imm = 1;
366	regular = 1;
367	load = 0;
368	len = 2;
369	sign_ext = 0;
370	break;
371	case 0x1A: /* SHI.bi */
372	imm = 1;
373	regular = 0;
374	load = 0;
375	len = 2;
376	sign_ext = 0;
377	break;
378	case 0x14: /* SWI */
379	imm = 1;
380	regular = 1;
381	load = 0;
382	len = 4;
383	sign_ext = 0;
384	break;
385	case 0x1C: /* SWI.bi */
386	imm = 1;
387	regular = 0;
388	load = 0;
389	len = 4;
390	sign_ext = 0;
391	break;
392
393	default:
394	switch (inst & 0xff) {
395
396	case 0x01: /* LH */
397	imm = 0;
398	regular = 1;
399	load = 1;
400	len = 2;
401	sign_ext = 0;
402	break;
403	case 0x05: /* LH.bi */
404	imm = 0;
405	regular = 0;
406	load = 1;
407	len = 2;
408	sign_ext = 0;
409	break;
410	case 0x11: /* LHS */
411	imm = 0;
412	regular = 1;
413	load = 1;
414	len = 2;
415	sign_ext = 1;
416	break;
417	case 0x15: /* LHS.bi */
418	imm = 0;
419	regular = 0;
420	load = 1;
421	len = 2;
422	sign_ext = 1;
423	break;
424	case 0x02: /* LW */
425	imm = 0;
426	regular = 1;
427	load = 1;
428	len = 4;
429	sign_ext = 0;
430	break;
431	case 0x06: /* LW.bi */
432	imm = 0;
433	regular = 0;
434	load = 1;
435	len = 4;
436	sign_ext = 0;
437	break;
438	case 0x09: /* SH */
439	imm = 0;
440	regular = 1;
441	load = 0;
442	len = 2;
443	sign_ext = 0;
444	break;
445	case 0x0D: /* SH.bi */
446	imm = 0;
447	regular = 0;
448	load = 0;
449	len = 2;
450	sign_ext = 0;
451	break;
452	case 0x0A: /* SW */
453	imm = 0;
454	regular = 1;
455	load = 0;
456	len = 4;
457	sign_ext = 0;
458	break;
459	case 0x0E: /* SW.bi */
460	imm = 0;
461	regular = 0;
462	load = 0;
463	len = 4;
464	sign_ext = 0;
465	break;
466
467	default:
468	return -EFAULT;
469	}
470	}
471
472	if (imm)
1613de8a	473	shift = GET_IMMSVAL(IMM(inst)) * len;
2923f5ea GH	474	else
	475	shift = *idx_to_addr(regs, RB(inst)) << SV(inst);
	476
	477	if (regular)
	478	unaligned_addr += shift;
	479
	480	if (load) {
	481
96d4f267	482	if (!access_ok((void *)unaligned_addr, len))
2923f5ea GH	483	return -EACCES;
	484
	485	get_data(unaligned_addr, &target_val, len);
	486
	487	if (sign_ext)
	488	*idx_to_addr(regs, RT(inst)) =
	489	sign_extend(target_val, len);
	490	else
	491	*idx_to_addr(regs, RT(inst)) = target_val;
	492	} else {
	493
96d4f267	494	if (!access_ok((void *)unaligned_addr, len))
2923f5ea GH	495	return -EACCES;
	496
	497	target_val = *idx_to_addr(regs, RT(inst));
	498	set_data((void *)unaligned_addr, target_val, len);
	499	}
	500
	501	if (!regular)
	502	*idx_to_addr(regs, RA(inst)) = unaligned_addr + shift;
	503
	504	regs->ipc += 4;
	505
	506	return 0;
	507	fault:
	508	return -EACCES;
	509	}
	510
	511	int do_unaligned_access(unsigned long addr, struct pt_regs *regs)
	512	{
	513	unsigned long inst;
	514	int ret = -EFAULT;
	515	mm_segment_t seg = get_fs();
	516
	517	inst = get_inst(regs->ipc);
	518
	519	DEBUG((unalign_access_debug > 0), 1,
	520	"Faulting addr: 0x%08lx, pc: 0x%08lx [inst: 0x%08lx ]\n", addr,
	521	regs->ipc, inst);
	522
	523	set_fs(USER_DS);
	524
	525	if (inst & NDS32_16BIT_INSTRUCTION)
	526	ret = do_16((inst >> 16) & 0xffff, regs);
	527	else
	528	ret = do_32(inst, regs);
	529	set_fs(seg);
	530
	531	return ret;
	532	}
	533
	534	#ifdef CONFIG_PROC_FS
	535
	536	static struct ctl_table alignment_tbl[3] = {
	537	{
	538	.procname = "enable",
	539	.data = &unalign_access_mode,
	540	.maxlen = sizeof(unalign_access_mode),
	541	.mode = 0666,
	542	.proc_handler = &proc_dointvec
	543	}
	544	,
	545	{
	546	.procname = "debug_info",
	547	.data = &unalign_access_debug,
	548	.maxlen = sizeof(unalign_access_debug),
	549	.mode = 0644,
	550	.proc_handler = &proc_dointvec
	551	}
	552	,
	553	{}
	554	};
	555
	556	static struct ctl_table nds32_sysctl_table[2] = {
	557	{
b3a75846	558	.procname = "unaligned_access",
2923f5ea GH	559	.mode = 0555,
	560	.child = alignment_tbl},
	561	{}
	562	};
	563
	564	static struct ctl_path nds32_path[2] = {
	565	{.procname = "nds32"},
	566	{}
	567	};
	568
	569	/*
	570	* Initialize nds32 alignment-correction interface
	571	*/
	572	static int __init nds32_sysctl_init(void)
	573	{
	574	register_sysctl_paths(nds32_path, nds32_sysctl_table);
	575	return 0;
	576	}
	577
	578	__initcall(nds32_sysctl_init);
	579	#endif /* CONFIG_PROC_FS */