?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
RT-AC56 3.0.0.4.374.37 core
[tomato.git] / release / src-rt-6.x.4708 / linux / linux-2.6.36 / arch / m32r / kernel / align.c
bloba6903508d87a417a2d61d31573f59045ffc9bddb
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
10 static 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
26 static 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
81 static 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
94 static 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
103 static 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
116 static 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
136 static 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
148 static 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
158 static 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
168 static 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
179 static 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
189 static 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
199 static 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
209 static 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
219 static 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
231 static 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
243 static 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
262 static 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
274 static 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
292 static 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
312 static 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
332 static 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
342 static 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
352 static 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
459 static 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
486 static 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
520 static 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
562 int 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 }
Tomato firmware and modifications.