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
 13void __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 */
 25int 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
 97void 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
127	force_sig_fault(sig, code, (void __user *)regs->pc);
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
154void 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
216void 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}

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