PageRenderTime 41ms CodeModel.GetById 14ms app.highlight 23ms RepoModel.GetById 1ms app.codeStats 0ms

/arch/m32r/kernel/align.c

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