[linux-2.6-block.git] / arch / nds32 / kernel / fpu.c

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

#include <linux/sched.h>
#include <linux/signal.h>
#include <linux/sched/signal.h>
#include <asm/processor.h>
#include <asm/user.h>
#include <asm/io.h>
#include <asm/bitfield.h>
#include <asm/fpu.h>

const struct fpu_struct init_fpuregs = {
	.fd_regs = {[0 ... 31] = sNAN64},
	.fpcsr = FPCSR_INIT,
#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
	.UDF_IEX_trap = 0
#endif
};

void save_fpu(struct task_struct *tsk)
{
	unsigned int fpcfg, fpcsr;

	enable_fpu();
	fpcfg = ((__nds32__fmfcfg() & FPCFG_mskFREG) >> FPCFG_offFREG);
	switch (fpcfg) {
	case SP32_DP32_reg:
		asm volatile ("fsdi $fd31, [%0+0xf8]\n\t"
			      "fsdi $fd30, [%0+0xf0]\n\t"
			      "fsdi $fd29, [%0+0xe8]\n\t"
			      "fsdi $fd28, [%0+0xe0]\n\t"
			      "fsdi $fd27, [%0+0xd8]\n\t"
			      "fsdi $fd26, [%0+0xd0]\n\t"
			      "fsdi $fd25, [%0+0xc8]\n\t"
			      "fsdi $fd24, [%0+0xc0]\n\t"
			      "fsdi $fd23, [%0+0xb8]\n\t"
			      "fsdi $fd22, [%0+0xb0]\n\t"
			      "fsdi $fd21, [%0+0xa8]\n\t"
			      "fsdi $fd20, [%0+0xa0]\n\t"
			      "fsdi $fd19, [%0+0x98]\n\t"
			      "fsdi $fd18, [%0+0x90]\n\t"
			      "fsdi $fd17, [%0+0x88]\n\t"
			      "fsdi $fd16, [%0+0x80]\n\t"
			      :	/* no output */
			      : "r" (&tsk->thread.fpu)
			      : "memory");
		/* fall through */
	case SP32_DP16_reg:
		asm volatile ("fsdi $fd15, [%0+0x78]\n\t"
			      "fsdi $fd14, [%0+0x70]\n\t"
			      "fsdi $fd13, [%0+0x68]\n\t"
			      "fsdi $fd12, [%0+0x60]\n\t"
			      "fsdi $fd11, [%0+0x58]\n\t"
			      "fsdi $fd10, [%0+0x50]\n\t"
			      "fsdi $fd9,  [%0+0x48]\n\t"
			      "fsdi $fd8,  [%0+0x40]\n\t"
			      :	/* no output */
			      : "r" (&tsk->thread.fpu)
			      : "memory");
		/* fall through */
	case SP16_DP8_reg:
		asm volatile ("fsdi $fd7,  [%0+0x38]\n\t"
			      "fsdi $fd6,  [%0+0x30]\n\t"
			      "fsdi $fd5,  [%0+0x28]\n\t"
			      "fsdi $fd4,  [%0+0x20]\n\t"
			      :	/* no output */
			      : "r" (&tsk->thread.fpu)
			      : "memory");
		/* fall through */
	case SP8_DP4_reg:
		asm volatile ("fsdi $fd3,  [%1+0x18]\n\t"
			      "fsdi $fd2,  [%1+0x10]\n\t"
			      "fsdi $fd1,  [%1+0x8]\n\t"
			      "fsdi $fd0,  [%1+0x0]\n\t"
			      "fmfcsr	%0\n\t"
			      "swi  %0, [%1+0x100]\n\t"
			      : "=&r" (fpcsr)
			      : "r"(&tsk->thread.fpu)
			      : "memory");
	}
	disable_fpu();
}

void load_fpu(const struct fpu_struct *fpregs)
{
	unsigned int fpcfg, fpcsr;

	enable_fpu();
	fpcfg = ((__nds32__fmfcfg() & FPCFG_mskFREG) >> FPCFG_offFREG);
	switch (fpcfg) {
	case SP32_DP32_reg:
		asm volatile ("fldi $fd31, [%0+0xf8]\n\t"
			      "fldi $fd30, [%0+0xf0]\n\t"
			      "fldi $fd29, [%0+0xe8]\n\t"
			      "fldi $fd28, [%0+0xe0]\n\t"
			      "fldi $fd27, [%0+0xd8]\n\t"
			      "fldi $fd26, [%0+0xd0]\n\t"
			      "fldi $fd25, [%0+0xc8]\n\t"
			      "fldi $fd24, [%0+0xc0]\n\t"
			      "fldi $fd23, [%0+0xb8]\n\t"
			      "fldi $fd22, [%0+0xb0]\n\t"
			      "fldi $fd21, [%0+0xa8]\n\t"
			      "fldi $fd20, [%0+0xa0]\n\t"
			      "fldi $fd19, [%0+0x98]\n\t"
			      "fldi $fd18, [%0+0x90]\n\t"
			      "fldi $fd17, [%0+0x88]\n\t"
			      "fldi $fd16, [%0+0x80]\n\t"
			      :	/* no output */
			      : "r" (fpregs));
		/* fall through */
	case SP32_DP16_reg:
		asm volatile ("fldi $fd15, [%0+0x78]\n\t"
			      "fldi $fd14, [%0+0x70]\n\t"
			      "fldi $fd13, [%0+0x68]\n\t"
			      "fldi $fd12, [%0+0x60]\n\t"
			      "fldi $fd11, [%0+0x58]\n\t"
			      "fldi $fd10, [%0+0x50]\n\t"
			      "fldi $fd9,  [%0+0x48]\n\t"
			      "fldi $fd8,  [%0+0x40]\n\t"
			      :	/* no output */
			      : "r" (fpregs));
		/* fall through */
	case SP16_DP8_reg:
		asm volatile ("fldi $fd7,  [%0+0x38]\n\t"
			      "fldi $fd6,  [%0+0x30]\n\t"
			      "fldi $fd5,  [%0+0x28]\n\t"
			      "fldi $fd4,  [%0+0x20]\n\t"
			      :	/* no output */
			      : "r" (fpregs));
		/* fall through */
	case SP8_DP4_reg:
		asm volatile ("fldi $fd3,  [%1+0x18]\n\t"
			      "fldi $fd2,  [%1+0x10]\n\t"
			      "fldi $fd1,  [%1+0x8]\n\t"
			      "fldi $fd0,  [%1+0x0]\n\t"
			      "lwi  %0, [%1+0x100]\n\t"
			      "fmtcsr	%0\n\t":"=&r" (fpcsr)
			      : "r"(fpregs));
	}
	disable_fpu();
}
void store_fpu_for_suspend(void)
{
#ifdef CONFIG_LAZY_FPU
	if (last_task_used_math != NULL)
		save_fpu(last_task_used_math);
	last_task_used_math = NULL;
#else
	if (!used_math())
		return;
	unlazy_fpu(current);
#endif
	clear_fpu(task_pt_regs(current));
}
inline void do_fpu_context_switch(struct pt_regs *regs)
{
	/* Enable to use FPU. */

	if (!user_mode(regs)) {
		pr_err("BUG: FPU is used in kernel mode.\n");
		BUG();
		return;
	}

	enable_ptreg_fpu(regs);
#ifdef CONFIG_LAZY_FPU	//Lazy FPU is used
	if (last_task_used_math == current)
		return;
	if (last_task_used_math != NULL)
		/* Other processes fpu state, save away */
		save_fpu(last_task_used_math);
	last_task_used_math = current;
#endif
	if (used_math()) {
		load_fpu(&current->thread.fpu);
	} else {
		/* First time FPU user.  */
		load_fpu(&init_fpuregs);
#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
		current->thread.fpu.UDF_IEX_trap = init_fpuregs.UDF_IEX_trap;
#endif
		set_used_math();
	}

}

inline void fill_sigfpe_signo(unsigned int fpcsr, int *signo)
{
	if (fpcsr & FPCSR_mskOVFT)
		*signo = FPE_FLTOVF;
#ifndef CONFIG_SUPPORT_DENORMAL_ARITHMETIC
	else if (fpcsr & FPCSR_mskUDFT)
		*signo = FPE_FLTUND;
#endif
	else if (fpcsr & FPCSR_mskIVOT)
		*signo = FPE_FLTINV;
	else if (fpcsr & FPCSR_mskDBZT)
		*signo = FPE_FLTDIV;
	else if (fpcsr & FPCSR_mskIEXT)
		*signo = FPE_FLTRES;
}

inline void handle_fpu_exception(struct pt_regs *regs)
{
	unsigned int fpcsr;
	int si_code = 0, si_signo = SIGFPE;
#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
	unsigned long redo_except = FPCSR_mskDNIT|FPCSR_mskUDFT|FPCSR_mskIEXT;
#else
	unsigned long redo_except = FPCSR_mskDNIT;
#endif

	lose_fpu();
	fpcsr = current->thread.fpu.fpcsr;

	if (fpcsr & redo_except) {
		si_signo = do_fpuemu(regs, &current->thread.fpu);
		fpcsr = current->thread.fpu.fpcsr;
		if (!si_signo) {
			current->thread.fpu.fpcsr &= ~(redo_except);
			goto done;
		}
	} else if (fpcsr & FPCSR_mskRIT) {
		if (!user_mode(regs))
			do_exit(SIGILL);
		si_signo = SIGILL;
	}

	switch (si_signo) {
	case SIGFPE:
		fill_sigfpe_signo(fpcsr, &si_code);
		break;
	case SIGILL:
		show_regs(regs);
		si_code = ILL_COPROC;
		break;
	case SIGBUS:
		si_code = BUS_ADRERR;
		break;
	default:
		break;
	}

	force_sig_fault(si_signo, si_code,
			(void __user *)instruction_pointer(regs));
done:
	own_fpu();
}

bool do_fpu_exception(unsigned int subtype, struct pt_regs *regs)
{
	int done = true;
	/* Coprocessor disabled exception */
	if (subtype == FPU_DISABLE_EXCEPTION) {
		preempt_disable();
		do_fpu_context_switch(regs);
		preempt_enable();
	}
	/* Coprocessor exception such as underflow and overflow */
	else if (subtype == FPU_EXCEPTION)
		handle_fpu_exception(regs);
	else
		done = false;
	return done;
}
Commit	Line	Data
e46bf83c VC	1	// SPDX-License-Identifier: GPL-2.0
	2	// Copyright (C) 2005-2018 Andes Technology Corporation
	3
	4	#include <linux/sched.h>
	5	#include <linux/signal.h>
	6	#include <linux/sched/signal.h>
	7	#include <asm/processor.h>
	8	#include <asm/user.h>
	9	#include <asm/io.h>
	10	#include <asm/bitfield.h>
	11	#include <asm/fpu.h>
	12
	13	const struct fpu_struct init_fpuregs = {
	14	.fd_regs = {[0 ... 31] = sNAN64},
44e92e03 VC	15	.fpcsr = FPCSR_INIT,
44e92e03 VC	16	#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
ed32949e	17	.UDF_IEX_trap = 0
44e92e03	18	#endif
e46bf83c VC	19	};
	20
	21	void save_fpu(struct task_struct *tsk)
	22	{
	23	unsigned int fpcfg, fpcsr;
	24
	25	enable_fpu();
	26	fpcfg = ((__nds32__fmfcfg() & FPCFG_mskFREG) >> FPCFG_offFREG);
	27	switch (fpcfg) {
	28	case SP32_DP32_reg:
	29	asm volatile ("fsdi $fd31, [%0+0xf8]\n\t"
	30	"fsdi $fd30, [%0+0xf0]\n\t"
	31	"fsdi $fd29, [%0+0xe8]\n\t"
	32	"fsdi $fd28, [%0+0xe0]\n\t"
	33	"fsdi $fd27, [%0+0xd8]\n\t"
	34	"fsdi $fd26, [%0+0xd0]\n\t"
	35	"fsdi $fd25, [%0+0xc8]\n\t"
	36	"fsdi $fd24, [%0+0xc0]\n\t"
	37	"fsdi $fd23, [%0+0xb8]\n\t"
	38	"fsdi $fd22, [%0+0xb0]\n\t"
	39	"fsdi $fd21, [%0+0xa8]\n\t"
	40	"fsdi $fd20, [%0+0xa0]\n\t"
	41	"fsdi $fd19, [%0+0x98]\n\t"
	42	"fsdi $fd18, [%0+0x90]\n\t"
	43	"fsdi $fd17, [%0+0x88]\n\t"
	44	"fsdi $fd16, [%0+0x80]\n\t"
	45	: /* no output */
	46	: "r" (&tsk->thread.fpu)
	47	: "memory");
	48	/* fall through */
	49	case SP32_DP16_reg:
	50	asm volatile ("fsdi $fd15, [%0+0x78]\n\t"
	51	"fsdi $fd14, [%0+0x70]\n\t"
	52	"fsdi $fd13, [%0+0x68]\n\t"
	53	"fsdi $fd12, [%0+0x60]\n\t"
	54	"fsdi $fd11, [%0+0x58]\n\t"
	55	"fsdi $fd10, [%0+0x50]\n\t"
	56	"fsdi $fd9, [%0+0x48]\n\t"
	57	"fsdi $fd8, [%0+0x40]\n\t"
	58	: /* no output */
	59	: "r" (&tsk->thread.fpu)
	60	: "memory");
	61	/* fall through */
	62	case SP16_DP8_reg:
	63	asm volatile ("fsdi $fd7, [%0+0x38]\n\t"
	64	"fsdi $fd6, [%0+0x30]\n\t"
	65	"fsdi $fd5, [%0+0x28]\n\t"
	66	"fsdi $fd4, [%0+0x20]\n\t"
	67	: /* no output */
	68	: "r" (&tsk->thread.fpu)
	69	: "memory");
	70	/* fall through */
	71	case SP8_DP4_reg:
	72	asm volatile ("fsdi $fd3, [%1+0x18]\n\t"
	73	"fsdi $fd2, [%1+0x10]\n\t"
	74	"fsdi $fd1, [%1+0x8]\n\t"
	75	"fsdi $fd0, [%1+0x0]\n\t"
	76	"fmfcsr %0\n\t"
	77	"swi %0, [%1+0x100]\n\t"
	78	: "=&r" (fpcsr)
	79	: "r"(&tsk->thread.fpu)
	80	: "memory");
	81	}
	82	disable_fpu();
83	}
84
85	void load_fpu(const struct fpu_struct *fpregs)
86	{
87	unsigned int fpcfg, fpcsr;
88
89	enable_fpu();
90	fpcfg = ((__nds32__fmfcfg() & FPCFG_mskFREG) >> FPCFG_offFREG);
91	switch (fpcfg) {
92	case SP32_DP32_reg:
93	asm volatile ("fldi $fd31, [%0+0xf8]\n\t"
94	"fldi $fd30, [%0+0xf0]\n\t"
95	"fldi $fd29, [%0+0xe8]\n\t"
96	"fldi $fd28, [%0+0xe0]\n\t"
97	"fldi $fd27, [%0+0xd8]\n\t"
98	"fldi $fd26, [%0+0xd0]\n\t"
99	"fldi $fd25, [%0+0xc8]\n\t"
100	"fldi $fd24, [%0+0xc0]\n\t"
101	"fldi $fd23, [%0+0xb8]\n\t"
102	"fldi $fd22, [%0+0xb0]\n\t"
103	"fldi $fd21, [%0+0xa8]\n\t"
104	"fldi $fd20, [%0+0xa0]\n\t"
105	"fldi $fd19, [%0+0x98]\n\t"
106	"fldi $fd18, [%0+0x90]\n\t"
107	"fldi $fd17, [%0+0x88]\n\t"
108	"fldi $fd16, [%0+0x80]\n\t"
109	: /* no output */
110	: "r" (fpregs));
111	/* fall through */
112	case SP32_DP16_reg:
113	asm volatile ("fldi $fd15, [%0+0x78]\n\t"
114	"fldi $fd14, [%0+0x70]\n\t"
115	"fldi $fd13, [%0+0x68]\n\t"
116	"fldi $fd12, [%0+0x60]\n\t"
117	"fldi $fd11, [%0+0x58]\n\t"
118	"fldi $fd10, [%0+0x50]\n\t"
119	"fldi $fd9, [%0+0x48]\n\t"
120	"fldi $fd8, [%0+0x40]\n\t"
121	: /* no output */
122	: "r" (fpregs));
123	/* fall through */
124	case SP16_DP8_reg:
125	asm volatile ("fldi $fd7, [%0+0x38]\n\t"
126	"fldi $fd6, [%0+0x30]\n\t"
127	"fldi $fd5, [%0+0x28]\n\t"
128	"fldi $fd4, [%0+0x20]\n\t"
129	: /* no output */
130	: "r" (fpregs));
131	/* fall through */
132	case SP8_DP4_reg:
133	asm volatile ("fldi $fd3, [%1+0x18]\n\t"
134	"fldi $fd2, [%1+0x10]\n\t"
135	"fldi $fd1, [%1+0x8]\n\t"
136	"fldi $fd0, [%1+0x0]\n\t"
137	"lwi %0, [%1+0x100]\n\t"
138	"fmtcsr %0\n\t":"=&r" (fpcsr)
139	: "r"(fpregs));
140	}
141	disable_fpu();
142	}
143	void store_fpu_for_suspend(void)
144	{
145	#ifdef CONFIG_LAZY_FPU
146	if (last_task_used_math != NULL)
147	save_fpu(last_task_used_math);
148	last_task_used_math = NULL;
149	#else
150	if (!used_math())
151	return;
152	unlazy_fpu(current);
153	#endif
154	clear_fpu(task_pt_regs(current));
155	}
156	inline void do_fpu_context_switch(struct pt_regs *regs)
157	{
158	/* Enable to use FPU. */
159
160	if (!user_mode(regs)) {
161	pr_err("BUG: FPU is used in kernel mode.\n");
162	BUG();
163	return;
164	}
165
166	enable_ptreg_fpu(regs);
167	#ifdef CONFIG_LAZY_FPU //Lazy FPU is used
168	if (last_task_used_math == current)
169	return;
170	if (last_task_used_math != NULL)
171	/* Other processes fpu state, save away */
172	save_fpu(last_task_used_math);
173	last_task_used_math = current;
174	#endif
175	if (used_math()) {
176	load_fpu(&current->thread.fpu);
177	} else {
178	/* First time FPU user. */
179	load_fpu(&init_fpuregs);
44e92e03	180	#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
ed32949e	181	current->thread.fpu.UDF_IEX_trap = init_fpuregs.UDF_IEX_trap;
44e92e03	182	#endif
e46bf83c VC	183	set_used_math();
	184	}
	185
	186	}
	187
	188	inline void fill_sigfpe_signo(unsigned int fpcsr, int *signo)
	189	{
	190	if (fpcsr & FPCSR_mskOVFT)
	191	*signo = FPE_FLTOVF;
44e92e03	192	#ifndef CONFIG_SUPPORT_DENORMAL_ARITHMETIC
1ac83250 VC	193	else if (fpcsr & FPCSR_mskUDFT)
1ac83250 VC	194	*signo = FPE_FLTUND;
44e92e03 VC	195	#endif
	196	else if (fpcsr & FPCSR_mskIVOT)
	197	*signo = FPE_FLTINV;
e46bf83c VC	198	else if (fpcsr & FPCSR_mskDBZT)
	199	*signo = FPE_FLTDIV;
	200	else if (fpcsr & FPCSR_mskIEXT)
	201	*signo = FPE_FLTRES;
	202	}
	203
	204	inline void handle_fpu_exception(struct pt_regs *regs)
	205	{
	206	unsigned int fpcsr;
	207	int si_code = 0, si_signo = SIGFPE;
44e92e03	208	#if IS_ENABLED(CONFIG_SUPPORT_DENORMAL_ARITHMETIC)
ed32949e	209	unsigned long redo_except = FPCSR_mskDNIT\|FPCSR_mskUDFT\|FPCSR_mskIEXT;
44e92e03 VC	210	#else
	211	unsigned long redo_except = FPCSR_mskDNIT;
	212	#endif
e46bf83c VC	213
	214	lose_fpu();
	215	fpcsr = current->thread.fpu.fpcsr;
	216
44e92e03	217	if (fpcsr & redo_except) {
1ac83250 VC	218	si_signo = do_fpuemu(regs, &current->thread.fpu);
1ac83250 VC	219	fpcsr = current->thread.fpu.fpcsr;
ed32949e VC	220	if (!si_signo) {
ed32949e VC	221	current->thread.fpu.fpcsr &= ~(redo_except);
1ac83250	222	goto done;
ed32949e	223	}
1ac83250	224	} else if (fpcsr & FPCSR_mskRIT) {
e46bf83c VC	225	if (!user_mode(regs))
	226	do_exit(SIGILL);
	227	si_signo = SIGILL;
1ac83250 VC	228	}
1ac83250 VC	229
1ac83250 VC	230	switch (si_signo) {
	231	case SIGFPE:
	232	fill_sigfpe_signo(fpcsr, &si_code);
	233	break;
	234	case SIGILL:
e46bf83c VC	235	show_regs(regs);
e46bf83c VC	236	si_code = ILL_COPROC;
1ac83250 VC	237	break;
	238	case SIGBUS:
	239	si_code = BUS_ADRERR;
	240	break;
	241	default:
	242	break;
	243	}
	244
e46bf83c	245	force_sig_fault(si_signo, si_code,
2e1661d2	246	(void __user *)instruction_pointer(regs));
1ac83250 VC	247	done:
1ac83250 VC	248	own_fpu();
e46bf83c VC	249	}
	250
	251	bool do_fpu_exception(unsigned int subtype, struct pt_regs *regs)
	252	{
	253	int done = true;
	254	/* Coprocessor disabled exception */
	255	if (subtype == FPU_DISABLE_EXCEPTION) {
	256	preempt_disable();
	257	do_fpu_context_switch(regs);
	258	preempt_enable();
	259	}
	260	/* Coprocessor exception such as underflow and overflow */
	261	else if (subtype == FPU_EXCEPTION)
	262	handle_fpu_exception(regs);
	263	else
	264	done = false;
	265	return done;
	266	}