[mirror_ubuntu-focal-kernel.git] / arch / csky / abiv2 / fpu.c

// SPDX-License-Identifier: GPL-2.0
// Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.

#include <linux/ptrace.h>
#include <linux/uaccess.h>
#include <abi/reg_ops.h>

#define MTCR_MASK	0xFC00FFE0
#define MFCR_MASK	0xFC00FFE0
#define MTCR_DIST	0xC0006420
#define MFCR_DIST	0xC0006020

void __init init_fpu(void)
{
	mtcr("cr<1, 2>", 0);
}

/*
 * fpu_libc_helper() is to help libc to excute:
 *  - mfcr %a, cr<1, 2>
 *  - mfcr %a, cr<2, 2>
 *  - mtcr %a, cr<1, 2>
 *  - mtcr %a, cr<2, 2>
 */
int fpu_libc_helper(struct pt_regs *regs)
{
	int fault;
	unsigned long instrptr, regx = 0;
	unsigned long index = 0, tmp = 0;
	unsigned long tinstr = 0;
	u16 instr_hi, instr_low;

	instrptr = instruction_pointer(regs);
	if (instrptr & 1)
		return 0;

	fault = __get_user(instr_low, (u16 *)instrptr);
	if (fault)
		return 0;

	fault = __get_user(instr_hi, (u16 *)(instrptr + 2));
	if (fault)
		return 0;

	tinstr = instr_hi | ((unsigned long)instr_low << 16);

	if (((tinstr >> 21) & 0x1F) != 2)
		return 0;

	if ((tinstr & MTCR_MASK) == MTCR_DIST) {
		index = (tinstr >> 16) & 0x1F;
		if (index > 13)
			return 0;

		tmp = tinstr & 0x1F;
		if (tmp > 2)
			return 0;

		regx =  *(&regs->a0 + index);

		if (tmp == 1)
			mtcr("cr<1, 2>", regx);
		else if (tmp == 2)
			mtcr("cr<2, 2>", regx);
		else
			return 0;

		regs->pc += 4;
		return 1;
	}

	if ((tinstr & MFCR_MASK) == MFCR_DIST) {
		index = tinstr & 0x1F;
		if (index > 13)
			return 0;

		tmp = ((tinstr >> 16) & 0x1F);
		if (tmp > 2)
			return 0;

		if (tmp == 1)
			regx = mfcr("cr<1, 2>");
		else if (tmp == 2)
			regx = mfcr("cr<2, 2>");
		else
			return 0;

		*(&regs->a0 + index) = regx;

		regs->pc += 4;
		return 1;
	}

	return 0;
}

void fpu_fpe(struct pt_regs *regs)
{
	int sig, code;
	unsigned int fesr;

	fesr = mfcr("cr<2, 2>");

	sig = SIGFPE;
	code = FPE_FLTUNK;

	if (fesr & FPE_ILLE) {
		sig = SIGILL;
		code = ILL_ILLOPC;
	} else if (fesr & FPE_IDC) {
		sig = SIGILL;
		code = ILL_ILLOPN;
	} else if (fesr & FPE_FEC) {
		sig = SIGFPE;
		if (fesr & FPE_IOC)
			code = FPE_FLTINV;
		else if (fesr & FPE_DZC)
			code = FPE_FLTDIV;
		else if (fesr & FPE_UFC)
			code = FPE_FLTUND;
		else if (fesr & FPE_OFC)
			code = FPE_FLTOVF;
		else if (fesr & FPE_IXC)
			code = FPE_FLTRES;
	}

	force_sig_fault(sig, code, (void __user *)regs->pc);
}

#define FMFVR_FPU_REGS(vrx, vry)	\
	"fmfvrl %0, "#vrx"\n"		\
	"fmfvrh %1, "#vrx"\n"		\
	"fmfvrl %2, "#vry"\n"		\
	"fmfvrh %3, "#vry"\n"

#define FMTVR_FPU_REGS(vrx, vry)	\
	"fmtvrl "#vrx", %0\n"		\
	"fmtvrh "#vrx", %1\n"		\
	"fmtvrl "#vry", %2\n"		\
	"fmtvrh "#vry", %3\n"

#define STW_FPU_REGS(a, b, c, d)	\
	"stw    %0, (%4, "#a")\n"	\
	"stw    %1, (%4, "#b")\n"	\
	"stw    %2, (%4, "#c")\n"	\
	"stw    %3, (%4, "#d")\n"

#define LDW_FPU_REGS(a, b, c, d)	\
	"ldw    %0, (%4, "#a")\n"	\
	"ldw    %1, (%4, "#b")\n"	\
	"ldw    %2, (%4, "#c")\n"	\
	"ldw    %3, (%4, "#d")\n"

void save_to_user_fp(struct user_fp *user_fp)
{
	unsigned long flg;
	unsigned long tmp1, tmp2;
	unsigned long *fpregs;

	local_irq_save(flg);

	tmp1 = mfcr("cr<1, 2>");
	tmp2 = mfcr("cr<2, 2>");

	user_fp->fcr = tmp1;
	user_fp->fesr = tmp2;

	fpregs = &user_fp->vr[0];
#ifdef CONFIG_CPU_HAS_FPUV2
#ifdef CONFIG_CPU_HAS_VDSP
	asm volatile(
		"vstmu.32    vr0-vr3,   (%0)\n"
		"vstmu.32    vr4-vr7,   (%0)\n"
		"vstmu.32    vr8-vr11,  (%0)\n"
		"vstmu.32    vr12-vr15, (%0)\n"
		"fstmu.64    vr16-vr31, (%0)\n"
		: "+a"(fpregs)
		::"memory");
#else
	asm volatile(
		"fstmu.64    vr0-vr31,  (%0)\n"
		: "+a"(fpregs)
		::"memory");
#endif
#else
	{
	unsigned long tmp3, tmp4;

	asm volatile(
		FMFVR_FPU_REGS(vr0, vr1)
		STW_FPU_REGS(0, 4, 16, 20)
		FMFVR_FPU_REGS(vr2, vr3)
		STW_FPU_REGS(32, 36, 48, 52)
		FMFVR_FPU_REGS(vr4, vr5)
		STW_FPU_REGS(64, 68, 80, 84)
		FMFVR_FPU_REGS(vr6, vr7)
		STW_FPU_REGS(96, 100, 112, 116)
		"addi	%4, 128\n"
		FMFVR_FPU_REGS(vr8, vr9)
		STW_FPU_REGS(0, 4, 16, 20)
		FMFVR_FPU_REGS(vr10, vr11)
		STW_FPU_REGS(32, 36, 48, 52)
		FMFVR_FPU_REGS(vr12, vr13)
		STW_FPU_REGS(64, 68, 80, 84)
		FMFVR_FPU_REGS(vr14, vr15)
		STW_FPU_REGS(96, 100, 112, 116)
		: "=a"(tmp1), "=a"(tmp2), "=a"(tmp3),
		  "=a"(tmp4), "+a"(fpregs)
		::"memory");
	}
#endif

	local_irq_restore(flg);
}

void restore_from_user_fp(struct user_fp *user_fp)
{
	unsigned long flg;
	unsigned long tmp1, tmp2;
	unsigned long *fpregs;

	local_irq_save(flg);

	tmp1 = user_fp->fcr;
	tmp2 = user_fp->fesr;

	mtcr("cr<1, 2>", tmp1);
	mtcr("cr<2, 2>", tmp2);

	fpregs = &user_fp->vr[0];
#ifdef CONFIG_CPU_HAS_FPUV2
#ifdef CONFIG_CPU_HAS_VDSP
	asm volatile(
		"vldmu.32    vr0-vr3,   (%0)\n"
		"vldmu.32    vr4-vr7,   (%0)\n"
		"vldmu.32    vr8-vr11,  (%0)\n"
		"vldmu.32    vr12-vr15, (%0)\n"
		"fldmu.64    vr16-vr31, (%0)\n"
		: "+a"(fpregs)
		::"memory");
#else
	asm volatile(
		"fldmu.64    vr0-vr31,  (%0)\n"
		: "+a"(fpregs)
		::"memory");
#endif
#else
	{
	unsigned long tmp3, tmp4;

	asm volatile(
		LDW_FPU_REGS(0, 4, 16, 20)
		FMTVR_FPU_REGS(vr0, vr1)
		LDW_FPU_REGS(32, 36, 48, 52)
		FMTVR_FPU_REGS(vr2, vr3)
		LDW_FPU_REGS(64, 68, 80, 84)
		FMTVR_FPU_REGS(vr4, vr5)
		LDW_FPU_REGS(96, 100, 112, 116)
		FMTVR_FPU_REGS(vr6, vr7)
		"addi	%4, 128\n"
		LDW_FPU_REGS(0, 4, 16, 20)
		FMTVR_FPU_REGS(vr8, vr9)
		LDW_FPU_REGS(32, 36, 48, 52)
		FMTVR_FPU_REGS(vr10, vr11)
		LDW_FPU_REGS(64, 68, 80, 84)
		FMTVR_FPU_REGS(vr12, vr13)
		LDW_FPU_REGS(96, 100, 112, 116)
		FMTVR_FPU_REGS(vr14, vr15)
		: "=a"(tmp1), "=a"(tmp2), "=a"(tmp3),
		  "=a"(tmp4), "+a"(fpregs)
		::"memory");
	}
#endif
	local_irq_restore(flg);
}
Commit	Line	Data
e9564df7 GR	1	// SPDX-License-Identifier: GPL-2.0
	2	// Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd.
	3
	4	#include <linux/ptrace.h>
	5	#include <linux/uaccess.h>
	6	#include <abi/reg_ops.h>
	7
	8	#define MTCR_MASK 0xFC00FFE0
	9	#define MFCR_MASK 0xFC00FFE0
	10	#define MTCR_DIST 0xC0006420
	11	#define MFCR_DIST 0xC0006020
	12
	13	void __init init_fpu(void)
	14	{
	15	mtcr("cr<1, 2>", 0);
	16	}
	17
	18	/*
	19	* fpu_libc_helper() is to help libc to excute:
	20	* - mfcr %a, cr<1, 2>
	21	* - mfcr %a, cr<2, 2>
	22	* - mtcr %a, cr<1, 2>
	23	* - mtcr %a, cr<2, 2>
	24	*/
	25	int fpu_libc_helper(struct pt_regs *regs)
	26	{
	27	int fault;
	28	unsigned long instrptr, regx = 0;
	29	unsigned long index = 0, tmp = 0;
	30	unsigned long tinstr = 0;
	31	u16 instr_hi, instr_low;
	32
	33	instrptr = instruction_pointer(regs);
	34	if (instrptr & 1)
	35	return 0;
	36
	37	fault = __get_user(instr_low, (u16 *)instrptr);
	38	if (fault)
	39	return 0;
	40
	41	fault = __get_user(instr_hi, (u16 *)(instrptr + 2));
	42	if (fault)
	43	return 0;
	44
	45	tinstr = instr_hi \| ((unsigned long)instr_low << 16);
	46
	47	if (((tinstr >> 21) & 0x1F) != 2)
	48	return 0;
	49
	50	if ((tinstr & MTCR_MASK) == MTCR_DIST) {
	51	index = (tinstr >> 16) & 0x1F;
	52	if (index > 13)
	53	return 0;
	54
	55	tmp = tinstr & 0x1F;
	56	if (tmp > 2)
	57	return 0;
	58
	59	regx = *(&regs->a0 + index);
	60
	61	if (tmp == 1)
	62	mtcr("cr<1, 2>", regx);
	63	else if (tmp == 2)
	64	mtcr("cr<2, 2>", regx);
65	else
66	return 0;
67
68	regs->pc += 4;
69	return 1;
70	}
71
72	if ((tinstr & MFCR_MASK) == MFCR_DIST) {
73	index = tinstr & 0x1F;
74	if (index > 13)
75	return 0;
76
77	tmp = ((tinstr >> 16) & 0x1F);
78	if (tmp > 2)
79	return 0;
80
81	if (tmp == 1)
82	regx = mfcr("cr<1, 2>");
83	else if (tmp == 2)
84	regx = mfcr("cr<2, 2>");
85	else
86	return 0;
87
88	*(&regs->a0 + index) = regx;
89
90	regs->pc += 4;
91	return 1;
92	}
93
94	return 0;
95	}
96
97	void fpu_fpe(struct pt_regs *regs)
98	{
99	int sig, code;
100	unsigned int fesr;
101
102	fesr = mfcr("cr<2, 2>");
103
104	sig = SIGFPE;
105	code = FPE_FLTUNK;
106
107	if (fesr & FPE_ILLE) {
108	sig = SIGILL;
109	code = ILL_ILLOPC;
110	} else if (fesr & FPE_IDC) {
111	sig = SIGILL;
112	code = ILL_ILLOPN;
113	} else if (fesr & FPE_FEC) {
114	sig = SIGFPE;
115	if (fesr & FPE_IOC)
116	code = FPE_FLTINV;
117	else if (fesr & FPE_DZC)
118	code = FPE_FLTDIV;
119	else if (fesr & FPE_UFC)
120	code = FPE_FLTUND;
121	else if (fesr & FPE_OFC)
122	code = FPE_FLTOVF;
123	else if (fesr & FPE_IXC)
124	code = FPE_FLTRES;
125	}
126
2e1661d2	127	force_sig_fault(sig, code, (void __user *)regs->pc);
e9564df7 GR	128	}
	129
	130	#define FMFVR_FPU_REGS(vrx, vry) \
	131	"fmfvrl %0, "#vrx"\n" \
	132	"fmfvrh %1, "#vrx"\n" \
	133	"fmfvrl %2, "#vry"\n" \
	134	"fmfvrh %3, "#vry"\n"
	135
	136	#define FMTVR_FPU_REGS(vrx, vry) \
	137	"fmtvrl "#vrx", %0\n" \
	138	"fmtvrh "#vrx", %1\n" \
	139	"fmtvrl "#vry", %2\n" \
	140	"fmtvrh "#vry", %3\n"
	141
	142	#define STW_FPU_REGS(a, b, c, d) \
	143	"stw %0, (%4, "#a")\n" \
	144	"stw %1, (%4, "#b")\n" \
	145	"stw %2, (%4, "#c")\n" \
	146	"stw %3, (%4, "#d")\n"
	147
	148	#define LDW_FPU_REGS(a, b, c, d) \
	149	"ldw %0, (%4, "#a")\n" \
	150	"ldw %1, (%4, "#b")\n" \
	151	"ldw %2, (%4, "#c")\n" \
	152	"ldw %3, (%4, "#d")\n"
	153
	154	void save_to_user_fp(struct user_fp *user_fp)
	155	{
	156	unsigned long flg;
	157	unsigned long tmp1, tmp2;
	158	unsigned long *fpregs;
	159
	160	local_irq_save(flg);
	161
	162	tmp1 = mfcr("cr<1, 2>");
	163	tmp2 = mfcr("cr<2, 2>");
	164
	165	user_fp->fcr = tmp1;
	166	user_fp->fesr = tmp2;
	167
	168	fpregs = &user_fp->vr[0];
	169	#ifdef CONFIG_CPU_HAS_FPUV2
	170	#ifdef CONFIG_CPU_HAS_VDSP
	171	asm volatile(
	172	"vstmu.32 vr0-vr3, (%0)\n"
	173	"vstmu.32 vr4-vr7, (%0)\n"
	174	"vstmu.32 vr8-vr11, (%0)\n"
	175	"vstmu.32 vr12-vr15, (%0)\n"
	176	"fstmu.64 vr16-vr31, (%0)\n"
	177	: "+a"(fpregs)
	178	::"memory");
	179	#else
	180	asm volatile(
	181	"fstmu.64 vr0-vr31, (%0)\n"
	182	: "+a"(fpregs)
	183	::"memory");
	184	#endif
	185	#else
	186	{
	187	unsigned long tmp3, tmp4;
	188
	189	asm volatile(
	190	FMFVR_FPU_REGS(vr0, vr1)
	191	STW_FPU_REGS(0, 4, 16, 20)
192	FMFVR_FPU_REGS(vr2, vr3)
193	STW_FPU_REGS(32, 36, 48, 52)
194	FMFVR_FPU_REGS(vr4, vr5)
195	STW_FPU_REGS(64, 68, 80, 84)
196	FMFVR_FPU_REGS(vr6, vr7)
197	STW_FPU_REGS(96, 100, 112, 116)
198	"addi %4, 128\n"
199	FMFVR_FPU_REGS(vr8, vr9)
200	STW_FPU_REGS(0, 4, 16, 20)
201	FMFVR_FPU_REGS(vr10, vr11)
202	STW_FPU_REGS(32, 36, 48, 52)
203	FMFVR_FPU_REGS(vr12, vr13)
204	STW_FPU_REGS(64, 68, 80, 84)
205	FMFVR_FPU_REGS(vr14, vr15)
206	STW_FPU_REGS(96, 100, 112, 116)
207	: "=a"(tmp1), "=a"(tmp2), "=a"(tmp3),
208	"=a"(tmp4), "+a"(fpregs)
209	::"memory");
210	}
211	#endif
212
213	local_irq_restore(flg);
214	}
215
216	void restore_from_user_fp(struct user_fp *user_fp)
217	{
218	unsigned long flg;
219	unsigned long tmp1, tmp2;
220	unsigned long *fpregs;
221
222	local_irq_save(flg);
223
224	tmp1 = user_fp->fcr;
225	tmp2 = user_fp->fesr;
226
227	mtcr("cr<1, 2>", tmp1);
228	mtcr("cr<2, 2>", tmp2);
229
230	fpregs = &user_fp->vr[0];
231	#ifdef CONFIG_CPU_HAS_FPUV2
232	#ifdef CONFIG_CPU_HAS_VDSP
233	asm volatile(
234	"vldmu.32 vr0-vr3, (%0)\n"
235	"vldmu.32 vr4-vr7, (%0)\n"
236	"vldmu.32 vr8-vr11, (%0)\n"
237	"vldmu.32 vr12-vr15, (%0)\n"
238	"fldmu.64 vr16-vr31, (%0)\n"
239	: "+a"(fpregs)
240	::"memory");
241	#else
242	asm volatile(
243	"fldmu.64 vr0-vr31, (%0)\n"
244	: "+a"(fpregs)
245	::"memory");
246	#endif
247	#else
248	{
249	unsigned long tmp3, tmp4;
250
251	asm volatile(
252	LDW_FPU_REGS(0, 4, 16, 20)
253	FMTVR_FPU_REGS(vr0, vr1)
254	LDW_FPU_REGS(32, 36, 48, 52)
255	FMTVR_FPU_REGS(vr2, vr3)
256	LDW_FPU_REGS(64, 68, 80, 84)
257	FMTVR_FPU_REGS(vr4, vr5)
258	LDW_FPU_REGS(96, 100, 112, 116)
259	FMTVR_FPU_REGS(vr6, vr7)
260	"addi %4, 128\n"
261	LDW_FPU_REGS(0, 4, 16, 20)
262	FMTVR_FPU_REGS(vr8, vr9)
263	LDW_FPU_REGS(32, 36, 48, 52)
264	FMTVR_FPU_REGS(vr10, vr11)
265	LDW_FPU_REGS(64, 68, 80, 84)
266	FMTVR_FPU_REGS(vr12, vr13)
267	LDW_FPU_REGS(96, 100, 112, 116)
268	FMTVR_FPU_REGS(vr14, vr15)
269	: "=a"(tmp1), "=a"(tmp2), "=a"(tmp3),
270	"=a"(tmp4), "+a"(fpregs)
271	::"memory");
272	}
273	#endif
274	local_irq_restore(flg);
275	}