1/*
2 * align.c - address exception handler for M32R
3 *
4 * Copyright (c) 2003 Hitoshi Yamamoto
5 */
6
7#include <asm/ptrace.h>
8#include <asm/uaccess.h>
9
10static int get_reg(struct pt_regs *regs, int nr)
11{
12	int val;
13
14	if (nr < 4)
15		val = *(unsigned long *)(&regs->r0 + nr);
16	else if (nr < 7)
17		val = *(unsigned long *)(&regs->r4 + (nr - 4));
18	else if (nr < 13)
19		val = *(unsigned long *)(&regs->r7 + (nr - 7));
20	else
21		val = *(unsigned long *)(&regs->fp + (nr - 13));
22
23	return val;
24}
25
26static void set_reg(struct pt_regs *regs, int nr, int val)
27{
28	if (nr < 4)
29		*(unsigned long *)(&regs->r0 + nr) = val;
30	else if (nr < 7)
31		*(unsigned long *)(&regs->r4 + (nr - 4)) = val;
32	else if (nr < 13)
33		*(unsigned long *)(&regs->r7 + (nr - 7)) = val;
34	else
35		*(unsigned long *)(&regs->fp + (nr - 13)) = val;
36}
37
38#define REG1(insn)	(((insn) & 0x0f00) >> 8)
39#define REG2(insn)	((insn) & 0x000f)
40#define PSW_BC		0x100
41
42/* O- instruction */
43#define ISA_LD1		0x20c0	/* ld Rdest, @Rsrc */
44#define ISA_LD2		0x20e0	/* ld Rdest, @Rsrc+ */
45#define ISA_LDH		0x20a0	/* ldh Rdest, @Rsrc */
46#define ISA_LDUH	0x20b0	/* lduh Rdest, @Rsrc */
47#define ISA_ST1		0x2040	/* st Rsrc1, @Rsrc2 */
48#define ISA_ST2		0x2060	/* st Rsrc1, @+Rsrc2 */
49#define ISA_ST3		0x2070	/* st Rsrc1, @-Rsrc2 */
50#define ISA_STH1	0x2020	/* sth Rsrc1, @Rsrc2 */
51#define ISA_STH2	0x2030	/* sth Rsrc1, @Rsrc2+ */
52
53#ifdef CONFIG_ISA_DUAL_ISSUE
54
55/* OS instruction */
56#define ISA_ADD		0x00a0	/* add Rdest, Rsrc */
57#define ISA_ADDI	0x4000	/* addi Rdest, #imm8 */
58#define ISA_ADDX	0x0090	/* addx Rdest, Rsrc */
59#define ISA_AND		0x00c0	/* and Rdest, Rsrc */
60#define ISA_CMP		0x0040	/* cmp Rsrc1, Rsrc2 */
61#define ISA_CMPEQ	0x0060	/* cmpeq Rsrc1, Rsrc2 */
62#define ISA_CMPU	0x0050	/* cmpu Rsrc1, Rsrc2 */
63#define ISA_CMPZ	0x0070	/* cmpz Rsrc */
64#define ISA_LDI		0x6000	/* ldi Rdest, #imm8 */
65#define ISA_MV		0x1080	/* mv Rdest, Rsrc */
66#define ISA_NEG		0x0030	/* neg Rdest, Rsrc */
67#define ISA_NOP		0x7000	/* nop */
68#define ISA_NOT		0x00b0	/* not Rdest, Rsrc */
69#define ISA_OR		0x00e0	/* or Rdest, Rsrc */
70#define ISA_SUB		0x0020	/* sub Rdest, Rsrc */
71#define ISA_SUBX	0x0010	/* subx Rdest, Rsrc */
72#define ISA_XOR		0x00d0	/* xor Rdest, Rsrc */
73
74/* -S instruction */
75#define ISA_MUL		0x1060	/* mul Rdest, Rsrc */
76#define ISA_MULLO_A0	0x3010	/* mullo Rsrc1, Rsrc2, A0 */
77#define ISA_MULLO_A1	0x3090	/* mullo Rsrc1, Rsrc2, A1 */
78#define ISA_MVFACMI_A0	0x50f2	/* mvfacmi Rdest, A0 */
79#define ISA_MVFACMI_A1	0x50f6	/* mvfacmi Rdest, A1 */
80
81static int emu_addi(unsigned short insn, struct pt_regs *regs)
82{
83	char imm = (char)(insn & 0xff);
84	int dest = REG1(insn);
85	int val;
86
87	val = get_reg(regs, dest);
88	val += imm;
89	set_reg(regs, dest, val);
90
91	return 0;
92}
93
94static int emu_ldi(unsigned short insn, struct pt_regs *regs)
95{
96	char imm = (char)(insn & 0xff);
97
98	set_reg(regs, REG1(insn), (int)imm);
99
100	return 0;
101}
102
103static int emu_add(unsigned short insn, struct pt_regs *regs)
104{
105	int dest = REG1(insn);
106	int src = REG2(insn);
107	int val;
108
109	val = get_reg(regs, dest);
110	val += get_reg(regs, src);
111	set_reg(regs, dest, val);
112
113	return 0;
114}
115
116static int emu_addx(unsigned short insn, struct pt_regs *regs)
117{
118	int dest = REG1(insn);
119	unsigned int val, tmp;
120
121	val = regs->psw & PSW_BC ? 1 : 0;
122	tmp = get_reg(regs, dest);
123	val += tmp;
124	val += (unsigned int)get_reg(regs, REG2(insn));
125	set_reg(regs, dest, val);
126
127	/* C bit set */
128	if (val < tmp)
129		regs->psw |= PSW_BC;
130	else
131		regs->psw &= ~(PSW_BC);
132
133	return 0;
134}
135
136static int emu_and(unsigned short insn, struct pt_regs *regs)
137{
138	int dest = REG1(insn);
139	int val;
140
141	val = get_reg(regs, dest);
142	val &= get_reg(regs, REG2(insn));
143	set_reg(regs, dest, val);
144
145	return 0;
146}
147
148static int emu_cmp(unsigned short insn, struct pt_regs *regs)
149{
150	if (get_reg(regs, REG1(insn)) < get_reg(regs, REG2(insn)))
151		regs->psw |= PSW_BC;
152	else
153		regs->psw &= ~(PSW_BC);
154
155	return 0;
156}
157
158static int emu_cmpeq(unsigned short insn, struct pt_regs *regs)
159{
160	if (get_reg(regs, REG1(insn)) == get_reg(regs, REG2(insn)))
161		regs->psw |= PSW_BC;
162	else
163		regs->psw &= ~(PSW_BC);
164
165	return 0;
166}
167
168static int emu_cmpu(unsigned short insn, struct pt_regs *regs)
169{
170	if ((unsigned int)get_reg(regs, REG1(insn))
171		< (unsigned int)get_reg(regs, REG2(insn)))
172		regs->psw |= PSW_BC;
173	else
174		regs->psw &= ~(PSW_BC);
175
176	return 0;
177}
178
179static int emu_cmpz(unsigned short insn, struct pt_regs *regs)
180{
181	if (!get_reg(regs, REG2(insn)))
182		regs->psw |= PSW_BC;
183	else
184		regs->psw &= ~(PSW_BC);
185
186	return 0;
187}
188
189static int emu_mv(unsigned short insn, struct pt_regs *regs)
190{
191	int val;
192
193	val = get_reg(regs, REG2(insn));
194	set_reg(regs, REG1(insn), val);
195
196	return 0;
197}
198
199static int emu_neg(unsigned short insn, struct pt_regs *regs)
200{
201	int val;
202
203	val = get_reg(regs, REG2(insn));
204	set_reg(regs, REG1(insn), 0 - val);
205
206	return 0;
207}
208
209static int emu_not(unsigned short insn, struct pt_regs *regs)
210{
211	int val;
212
213	val = get_reg(regs, REG2(insn));
214	set_reg(regs, REG1(insn), ~val);
215
216	return 0;
217}
218
219static int emu_or(unsigned short insn, struct pt_regs *regs)
220{
221	int dest = REG1(insn);
222	int val;
223
224	val = get_reg(regs, dest);
225	val |= get_reg(regs, REG2(insn));
226	set_reg(regs, dest, val);
227
228	return 0;
229}
230
231static int emu_sub(unsigned short insn, struct pt_regs *regs)
232{
233	int dest = REG1(insn);
234	int val;
235
236	val = get_reg(regs, dest);
237	val -= get_reg(regs, REG2(insn));
238	set_reg(regs, dest, val);
239
240	return 0;
241}
242
243static int emu_subx(unsigned short insn, struct pt_regs *regs)
244{
245	int dest = REG1(insn);
246	unsigned int val, tmp;
247
248	val = tmp = get_reg(regs, dest);
249	val -= (unsigned int)get_reg(regs, REG2(insn));
250	val -= regs->psw & PSW_BC ? 1 : 0;
251	set_reg(regs, dest, val);
252
253	/* C bit set */
254	if (val > tmp)
255		regs->psw |= PSW_BC;
256	else
257		regs->psw &= ~(PSW_BC);
258
259	return 0;
260}
261
262static int emu_xor(unsigned short insn, struct pt_regs *regs)
263{
264	int dest = REG1(insn);
265	unsigned int val;
266
267	val = (unsigned int)get_reg(regs, dest);
268	val ^= (unsigned int)get_reg(regs, REG2(insn));
269	set_reg(regs, dest, val);
270
271	return 0;
272}
273
274static int emu_mul(unsigned short insn, struct pt_regs *regs)
275{
276	int dest = REG1(insn);
277	int reg1, reg2;
278
279	reg1 = get_reg(regs, dest);
280	reg2 = get_reg(regs, REG2(insn));
281
282	__asm__ __volatile__ (
283		"mul	%0, %1;		\n\t"
284		: "+r" (reg1) : "r" (reg2)
285	);
286
287	set_reg(regs, dest, reg1);
288
289	return 0;
290}
291
292static int emu_mullo_a0(unsigned short insn, struct pt_regs *regs)
293{
294	int reg1, reg2;
295
296	reg1 = get_reg(regs, REG1(insn));
297	reg2 = get_reg(regs, REG2(insn));
298
299	__asm__ __volatile__ (
300		"mullo		%0, %1, a0;	\n\t"
301		"mvfachi	%0, a0;		\n\t"
302		"mvfaclo	%1, a0;		\n\t"
303		: "+r" (reg1), "+r" (reg2)
304	);
305
306	regs->acc0h = reg1;
307	regs->acc0l = reg2;
308
309	return 0;
310}
311
312static int emu_mullo_a1(unsigned short insn, struct pt_regs *regs)
313{
314	int reg1, reg2;
315
316	reg1 = get_reg(regs, REG1(insn));
317	reg2 = get_reg(regs, REG2(insn));
318
319	__asm__ __volatile__ (
320		"mullo		%0, %1, a0;	\n\t"
321		"mvfachi	%0, a0;		\n\t"
322		"mvfaclo	%1, a0;		\n\t"
323		: "+r" (reg1), "+r" (reg2)
324	);
325
326	regs->acc1h = reg1;
327	regs->acc1l = reg2;
328
329	return 0;
330}
331
332static int emu_mvfacmi_a0(unsigned short insn, struct pt_regs *regs)
333{
334	unsigned long val;
335
336	val = (regs->acc0h << 16) | (regs->acc0l >> 16);
337	set_reg(regs, REG1(insn), (int)val);
338
339	return 0;
340}
341
342static int emu_mvfacmi_a1(unsigned short insn, struct pt_regs *regs)
343{
344	unsigned long val;
345
346	val = (regs->acc1h << 16) | (regs->acc1l >> 16);
347	set_reg(regs, REG1(insn), (int)val);
348
349	return 0;
350}
351
352static int emu_m32r2(unsigned short insn, struct pt_regs *regs)
353{
354	int res = -1;
355
356	if ((insn & 0x7fff) == ISA_NOP)	/* nop */
357		return 0;
358
359	switch(insn & 0x7000) {
360	case ISA_ADDI:		/* addi Rdest, #imm8 */
361		res = emu_addi(insn, regs);
362		break;
363	case ISA_LDI:		/* ldi Rdest, #imm8 */
364		res = emu_ldi(insn, regs);
365		break;
366	default:
367		break;
368	}
369
370	if (!res)
371		return 0;
372
373	switch(insn & 0x70f0) {
374	case ISA_ADD:		/* add Rdest, Rsrc */
375		res = emu_add(insn, regs);
376		break;
377	case ISA_ADDX:		/* addx Rdest, Rsrc */
378		res = emu_addx(insn, regs);
379		break;
380	case ISA_AND:		/* and Rdest, Rsrc */
381		res = emu_and(insn, regs);
382		break;
383	case ISA_CMP:		/* cmp Rsrc1, Rsrc2 */
384		res = emu_cmp(insn, regs);
385		break;
386	case ISA_CMPEQ:		/* cmpeq Rsrc1, Rsrc2 */
387		res = emu_cmpeq(insn, regs);
388		break;
389	case ISA_CMPU:		/* cmpu Rsrc1, Rsrc2 */
390		res = emu_cmpu(insn, regs);
391		break;
392	case ISA_CMPZ:		/* cmpz Rsrc */
393		res = emu_cmpz(insn, regs);
394		break;
395	case ISA_MV:		/* mv Rdest, Rsrc */
396		res = emu_mv(insn, regs);
397		break;
398	case ISA_NEG:		/* neg Rdest, Rsrc */
399		res = emu_neg(insn, regs);
400		break;
401	case ISA_NOT:		/* not Rdest, Rsrc */
402		res = emu_not(insn, regs);
403		break;
404	case ISA_OR:		/* or Rdest, Rsrc */
405		res = emu_or(insn, regs);
406		break;
407	case ISA_SUB:		/* sub Rdest, Rsrc */
408		res = emu_sub(insn, regs);
409		break;
410	case ISA_SUBX:		/* subx Rdest, Rsrc */
411		res = emu_subx(insn, regs);
412		break;
413	case ISA_XOR:		/* xor Rdest, Rsrc */
414		res = emu_xor(insn, regs);
415		break;
416	case ISA_MUL:		/* mul Rdest, Rsrc */
417		res = emu_mul(insn, regs);
418		break;
419	case ISA_MULLO_A0:	/* mullo Rsrc1, Rsrc2 */
420		res = emu_mullo_a0(insn, regs);
421		break;
422	case ISA_MULLO_A1:	/* mullo Rsrc1, Rsrc2 */
423		res = emu_mullo_a1(insn, regs);
424		break;
425	default:
426		break;
427	}
428
429	if (!res)
430		return 0;
431
432	switch(insn & 0x70ff) {
433	case ISA_MVFACMI_A0:	/* mvfacmi Rdest */
434		res = emu_mvfacmi_a0(insn, regs);
435		break;
436	case ISA_MVFACMI_A1:	/* mvfacmi Rdest */
437		res = emu_mvfacmi_a1(insn, regs);
438		break;
439	default:
440		break;
441	}
442
443	return res;
444}
445
446#endif	/* CONFIG_ISA_DUAL_ISSUE */
447
448/*
449 * ld   : ?010 dest 1100 src
450 *        0010 dest 1110 src : ld Rdest, @Rsrc+
451 * ldh  : ?010 dest 1010 src
452 * lduh : ?010 dest 1011 src
453 * st   : ?010 src1 0100 src2
454 *        0010 src1 0110 src2 : st Rsrc1, @+Rsrc2
455 *        0010 src1 0111 src2 : st Rsrc1, @-Rsrc2
456 * sth  : ?010 src1 0010 src2
457 */
458
459static int insn_check(unsigned long insn, struct pt_regs *regs,
460	unsigned char **ucp)
461{
462	int res = 0;
463
464	/*
465	 * 32bit insn
466	 *  ld Rdest, @(disp16, Rsrc)
467	 *  st Rdest, @(disp16, Rsrc)
468	 */
469	if (insn & 0x80000000) {	/* 32bit insn */
470		*ucp += (short)(insn & 0x0000ffff);
471		regs->bpc += 4;
472	} else {			/* 16bit insn */
473#ifdef CONFIG_ISA_DUAL_ISSUE
474		/* parallel exec check */
475		if (!(regs->bpc & 0x2) && insn & 0x8000) {
476			res = emu_m32r2((unsigned short)insn, regs);
477			regs->bpc += 4;
478		} else
479#endif	/* CONFIG_ISA_DUAL_ISSUE */
480			regs->bpc += 2;
481	}
482
483	return res;
484}
485
486static int emu_ld(unsigned long insn32, struct pt_regs *regs)
487{
488	unsigned char *ucp;
489	unsigned long val;
490	unsigned short insn16;
491	int size, src;
492
493	insn16 = insn32 >> 16;
494	src = REG2(insn16);
495	ucp = (unsigned char *)get_reg(regs, src);
496
497	if (insn_check(insn32, regs, &ucp))
498		return -1;
499
500	size = insn16 & 0x0040 ? 4 : 2;
501	if (copy_from_user(&val, ucp, size))
502		return -1;
503
504	if (size == 2)
505		val >>= 16;
506
507	/* ldh sign check */
508	if ((insn16 & 0x00f0) == 0x00a0 && (val & 0x8000))
509		val |= 0xffff0000;
510
511	set_reg(regs, REG1(insn16), val);
512
513	/* ld increment check */
514	if ((insn16 & 0xf0f0) == ISA_LD2)	/* ld Rdest, @Rsrc+ */
515		set_reg(regs, src, (unsigned long)(ucp + 4));
516
517	return 0;
518}
519
520static int emu_st(unsigned long insn32, struct pt_regs *regs)
521{
522	unsigned char *ucp;
523	unsigned long val;
524	unsigned short insn16;
525	int size, src2;
526
527	insn16 = insn32 >> 16;
528	src2 = REG2(insn16);
529
530	ucp = (unsigned char *)get_reg(regs, src2);
531
532	if (insn_check(insn32, regs, &ucp))
533		return -1;
534
535	size = insn16 & 0x0040 ? 4 : 2;
536	val = get_reg(regs, REG1(insn16));
537	if (size == 2)
538		val <<= 16;
539
540	/* st inc/dec check */
541	if ((insn16 & 0xf0e0) == 0x2060) {
542		if (insn16 & 0x0010)
543			ucp -= 4;
544		else
545			ucp += 4;
546
547		set_reg(regs, src2, (unsigned long)ucp);
548	}
549
550	if (copy_to_user(ucp, &val, size))
551		return -1;
552
553	/* sth inc check */
554	if ((insn16 & 0xf0f0) == ISA_STH2) {
555		ucp += 2;
556		set_reg(regs, src2, (unsigned long)ucp);
557	}
558
559	return 0;
560}
561
562int handle_unaligned_access(unsigned long insn32, struct pt_regs *regs)
563{
564	unsigned short insn16;
565	int res;
566
567	insn16 = insn32 >> 16;
568
569	/* ld or st check */
570	if ((insn16 & 0x7000) != 0x2000)
571		return -1;
572
573	/* insn alignment check */
574	if ((insn16 & 0x8000) && (regs->bpc & 3))
575		return -1;
576
577	if (insn16 & 0x0080)	/* ld */
578		res = emu_ld(insn32, regs);
579	else			/* st */
580		res = emu_st(insn32, regs);
581
582	return res;
583}
584
585