[PATCH] voyager: fix the cpu_possible_map to make voyager boot again
[linux-2.6] / 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 <linux/config.h>
8 #include <asm/ptrace.h>
9 #include <asm/uaccess.h>
10
11 static 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
27 static 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
82 static 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
95 static 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
104 static 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
117 static 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
137 static 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
149 static 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
159 static 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
169 static 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
180 static 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
190 static 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
200 static 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
210 static 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
220 static 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
232 static 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
244 static 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
263 static 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
275 static 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
293 static 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
313 static 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
333 static 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
343 static 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
353 static 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
460 static 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
487 static 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
521 static 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
563 int 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