9p: move readn meta-function from client to fs layer
[linux-2.6.git] / arch / m32r / kernel / align.c
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 }
584