projects / openwrt / openwrt.git / blob
? search:


d856d061dcfa245277a49012de79fc97a7e0ca2e
[openwrt/openwrt.git] / target / linux / ubicom32 / files / arch / ubicom32 / kernel / unaligned_trap.c
1 /*
2 * arch/ubicom32/kernel/unaligned_trap.c
3 * Handle unaligned traps in both user or kernel space.
4 *
5 * (C) Copyright 2009, Ubicom, Inc.
6 *
7 * This file is part of the Ubicom32 Linux Kernel Port.
8 *
9 * The Ubicom32 Linux Kernel Port is free software: you can redistribute
10 * it and/or modify it under the terms of the GNU General Public License
11 * as published by the Free Software Foundation, either version 2 of the
12 * License, or (at your option) any later version.
13 *
14 * The Ubicom32 Linux Kernel Port is distributed in the hope that it
15 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
16 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
17 * the GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with the Ubicom32 Linux Kernel Port. If not,
21 * see <http://www.gnu.org/licenses/>.
22 *
23 * Ubicom32 implementation derived from (with many thanks):
24 * arch/m68knommu
25 * arch/blackfin
26 * arch/parisc
27 */
28
29 #include <linux/types.h>
30 #include <linux/kernel.h>
31 #include <asm/cacheflush.h>
32 #include <asm/traps.h>
33
34 #define FALSE 0
35 #define TRUE 1
36
37 /* no possible trap */
38 #define UNUSED 0
39 /* possible source operand trap */
40 #define SRC 1
41 #define SRC_2 2
42 /* possible destination operand trap */
43 #define DEST 3
44 #define DEST_2 4
45 /* can be either source or destination or both */
46 #define TWO_OP 5
47 #define TWO_OP_2 6
48
49 /* TODO: What is the real value here, put something in to make it compile for
50 * now */
51 #define MOVE_2 0x0d
52 #define LSL_2 0x11
53 #define LSR_2 0x13
54 #define MOVEI 0x19
55 #define CMPI 0x18
56
57 static int op_format[32] =
58 {
59 TWO_OP, /* 0x00 */
60 UNUSED,
61 SRC,
62 UNUSED,
63 TWO_OP, /* 0x04 */
64 TWO_OP,
65 SRC,
66 UNUSED,
67 TWO_OP_2, /* 0x08 */
68 TWO_OP,
69 TWO_OP_2,
70 TWO_OP,
71 TWO_OP_2, /* 0x0C */
72 TWO_OP,
73 TWO_OP_2,
74 TWO_OP,
75 TWO_OP, /* 0x10 */
76 TWO_OP_2,
77 TWO_OP,
78 TWO_OP,
79 UNUSED, /* 0x14 */
80 UNUSED,
81 UNUSED,
82 UNUSED,
83 SRC_2, /* 0x18 */
84 DEST_2,
85 UNUSED,
86 UNUSED,
87 UNUSED, /* 0x1C */
88 UNUSED,
89 UNUSED, /* unaligned CALLI will not be fixed. */
90 UNUSED
91 };
92
93 static int op_0_format[32] =
94 {
95 UNUSED, /* 0x00 */
96 UNUSED,
97 UNUSED,
98 UNUSED,
99 UNUSED, /* 0x04 - ret don't fix - bad ret is always wrong */
100 UNUSED,
101 UNUSED,
102 UNUSED,
103 UNUSED, /* 0x08 */
104 UNUSED,
105 TWO_OP,
106 TWO_OP_2,
107 TWO_OP, /* 0x0c */
108 TWO_OP_2,
109 TWO_OP,
110 UNUSED, /* .1 can't trap */
111 UNUSED, /* 0x10 */
112 UNUSED,
113 SRC,
114 UNUSED,
115 UNUSED, /* 0x14 */
116 TWO_OP_2,
117 UNUSED,
118 UNUSED,
119 UNUSED, /* 0x18 */
120 UNUSED,
121 UNUSED,
122 UNUSED,
123 DEST, /* 0x1c */
124 DEST,
125 DEST,
126 DEST, /* all lea have 32-bit destination */
127 };
128
129 static int op_2_format[32] =
130 {
131 UNUSED, /* 0x00 */
132 UNUSED,
133 UNUSED,
134 UNUSED,
135 UNUSED, /* 0x04 */
136 UNUSED,
137 SRC,
138 UNUSED,
139 UNUSED, /* 0x08 crcgen is .1 */
140 UNUSED,
141 UNUSED,
142 UNUSED,
143 UNUSED, /* 0x0c */
144 UNUSED,
145 UNUSED,
146 UNUSED,
147 SRC, /* 0x10 */
148 SRC_2,
149 SRC,
150 SRC_2,
151 SRC, /* 0x14 */
152 SRC_2,
153 SRC,
154 UNUSED,
155 UNUSED, /* 0x18 */
156 UNUSED,
157 SRC,
158 UNUSED,
159 SRC, /* 0x1c */
160 UNUSED,
161 SRC_2,
162 UNUSED,
163 };
164
165 static int op_6_format[32] =
166 {
167 SRC_2, /* 0x00 */
168 SRC_2,
169 SRC_2,
170 SRC_2,
171 SRC_2, /* 0x04 */
172 SRC_2,
173 UNUSED,
174 SRC_2,
175 SRC, /* 0x08 MULS.4 */
176 SRC_2,
177 SRC,
178 UNUSED,
179 UNUSED, /* 0x0c */
180 UNUSED,
181 UNUSED,
182 UNUSED,
183 SRC, /* 0x10 */
184 SRC_2,
185 SRC,
186 SRC_2,
187 UNUSED, /* 0x14 */
188 UNUSED,
189 UNUSED,
190 UNUSED,
191 UNUSED, /* 0x18 */
192 UNUSED,
193 UNUSED,
194 UNUSED,
195 UNUSED, /* 0x1c */
196 UNUSED,
197 UNUSED,
198 UNUSED,
199 };
200
201 /*
202 * unaligned_get_address()
203 * get an address using save_an and save_dn registers, and updates save_an
204 * with side effects
205 */
206 unsigned char *unaligned_get_address(int thread, int specifier, int four_byte,
207 unsigned int save_an[],
208 unsigned int save_dn[], int *write_back_an)
209 {
210 unsigned char *address;
211
212 int areg = (specifier >> 5) & 7;
213 if ((specifier >> 8) == 2) {
214 int offset = specifier & 0xf;
215 offset = ((offset << 28) >> 28);
216 if (likely(four_byte)) {
217 offset <<= 2;
218 } else {
219 offset <<= 1;
220 }
221 if (specifier & 0x10) {
222 address = (unsigned char *)(save_an[areg] + offset);
223 } else {
224 address = (unsigned char *)save_an[areg];
225 }
226 save_an[areg] = save_an[areg] + offset;
227
228 /*
229 * Let caller know An registers have been modified.
230 */
231 *write_back_an = 1;
232 } else if ((specifier >> 8) == 3) {
233 int dreg = specifier & 0xf;
234 if (likely(four_byte)) {
235 address = (unsigned char *)(save_an[areg] +
236 (save_dn[dreg] << 2));
237 } else {
238 address = (unsigned char *)(save_an[areg] +
239 (save_dn[dreg] << 1));
240 }
241 } else {
242 int offset = ((specifier >> 3) & 0x60) | (specifier & 0x1f);
243 if (likely(four_byte)) {
244 address = (unsigned char *)(save_an[areg] +
245 (offset << 2));
246 } else {
247 address = (unsigned char *)(save_an[areg] +
248 (offset << 1));
249 }
250 }
251
252 return address;
253 }
254
255 static int save_dn[16];
256 static int save_an[8];
257 static int save_acc[5];
258
259 /*
260 * unaligned_emulate()
261 * emulate the instruction at thread's pc that has taken an unaligned data
262 * trap.
263 *
264 * source or destination or both might be unaligned
265 * the instruction must have a memory source or destination or both
266 * the emulated instruction is copied and executed in this thread
267 *
268 * TODO: Protection is handled outside of this function
269 * TODO: handling simultaneous unaligned and memory protection traps
270 *
271 * Get thread state
272 * the PC and instruction (and local copy, emulate_inst), and An
273 * and Dn registers
274 * All implicit soruce state (source3, CSR, accumulators)
275
276 * if the instruction has a memory source
277 * Use the instruction, An and Dn registers to form src_address
278 * get unaligned source data from src_address (usually sign
279 * extended)
280 * (2 bytes, with or without sign extension, or 4 bytes)
281 * modify emulate_inst to use d0 as source
282 * else
283 * get the soure operand from one of thread's registers
284 * if instruction has a memory destination
285 * Use the instruction, An and Dn registers to form dest_address
286 * modify emulate_inst to use d0 as destination
287 * if there was a memory source
288 * put the source data in thread's d0
289 * get the source-2 Dn operand and source 3 operand from thread
290 * execute modified inst
291 * (save it, flush caches, set up local values for implicit
292 * sources, execute, save explicit and implicit results)
293 * if inst has destination address
294 * copy result to dest_address, possibly unaligned, 1, 2, or 4
295 * bytes
296 * restore thread's implicit results (modified address registers, CSR,
297 * accumulators) add 4 to thread's pc
298 */
299 void unaligned_emulate(unsigned int thread)
300 {
301 unsigned int pc;
302 unsigned int inst;
303 unsigned int op;
304 unsigned int subop;
305 int format;
306 unsigned int emulate_inst;
307 int four_byte;
308 int src_operand, dest_operand;
309 int save_csr;
310 int source3;
311 unsigned int source1;
312 unsigned int source_data;
313 unsigned char *dest_address = NULL;
314 int source2 = 0;
315 unsigned int result;
316 unsigned int write_back_an = 0;
317 unsigned int chip_id_copy;
318
319 extern unsigned int trap_emulate;
320 extern unsigned int ubicom32_emulate_insn(int source1, int source2,
321 int source3, int *save_acc,
322 int *save_csr);
323
324 /*
325 * get the chip_id
326 */
327 asm volatile (
328 " move.4 %0, chip_id \n\t" /* get chip_id. */
329 : "=r"(chip_id_copy)
330 :
331 );
332
333 /*
334 * get the pc
335 */
336 asm volatile (
337 " move.4 CSR, %1 \n\t" /* set source thread in
338 * CSR */
339 " setcsr_flush 0 \n\t"
340 " move.4 %0, pc \n\t"
341 " move.4 CSR, #0 \n\t" /* restore CSR */
342 " setcsr_flush 0 \n\t"
343 : "=a"(pc)
344 : "d" ((1 << 8) | (thread << 9))
345 : "cc"
346 );
347
348 inst = *((unsigned int *)pc);
349 op = inst >> 27;
350 if (unlikely(op == 2 || op == 6)) {
351 subop = (inst >> 21) & 0x1f;
352 } else {
353 subop = (inst >> 11) & 0x1f;
354 }
355 format = op_format[op];
356 emulate_inst = inst;
357
358 if (op == 0) {
359 format = op_0_format[subop];
360 } else if (op == 2) {
361 format = op_2_format[subop];
362 } else if (op == 6) {
363 format = op_6_format[subop];
364 }
365
366 if (unlikely(format == UNUSED)) {
367 /*
368 * We are not going to emulate this. Bump PC by 4 and move on.
369 */
370 asm volatile (
371 " move.4 CSR, %0 \n\t"
372 " setcsr_flush 0 \n\t"
373 " move.4 pc, %1 \n\t"
374 " setcsr #0 \n\t"
375 " setcsr_flush 0 \n\t"
376 :
377 : "d"((1 << 14) | (thread << 15)), "d"(pc + 4)
378 : "cc"
379 );
380 return;
381 }
382
383 four_byte = (format == TWO_OP || format == DEST || format == SRC);
384
385 /*
386 * source or destination memory operand needs emulation
387 */
388 src_operand = (format == SRC ||
389 format == SRC_2 ||
390 format == TWO_OP ||
391 format == TWO_OP_2) &&
392 ((inst >> 8) & 7) > 1;
393
394 dest_operand = (format == DEST ||
395 format == DEST_2 ||
396 format == TWO_OP ||
397 format == TWO_OP_2) &&
398 ((inst >> 24) & 7) > 1;
399
400 /*
401 * get thread's implicit sources (not covered by source context select).
402 * data and address registers and CSR (for flag bits) and src3 and
403 * accumulators
404 */
405 asm volatile (
406 " move.4 CSR, %2 \n\t" /* set source thread in
407 * CSR */
408 " setcsr_flush 0 \n\t"
409 " move.4 (%3), d0 \n\t" /* get dn registers */
410 " move.4 4(%3), d1 \n\t"
411 " move.4 8(%3), d2 \n\t"
412 " move.4 12(%3), d3 \n\t"
413 " move.4 16(%3), d4 \n\t"
414 " move.4 20(%3), d5 \n\t"
415 " move.4 24(%3), d6 \n\t"
416 " move.4 28(%3), d7 \n\t"
417 " move.4 32(%3), d8 \n\t"
418 " move.4 36(%3), d9 \n\t"
419 " move.4 40(%3), d10 \n\t"
420 " move.4 44(%3), d11 \n\t"
421 " move.4 48(%3), d12 \n\t"
422 " move.4 52(%3), d13 \n\t"
423 " move.4 56(%3), d14 \n\t"
424 " move.4 60(%3), d15 \n\t"
425 " move.4 (%4), a0 \n\t" /* get an registers */
426 " move.4 4(%4), a1 \n\t"
427 " move.4 8(%4), a2 \n\t"
428 " move.4 12(%4), a3 \n\t"
429 " move.4 16(%4), a4 \n\t"
430 " move.4 20(%4), a5 \n\t"
431 " move.4 24(%4), a6 \n\t"
432 " move.4 28(%4), a7 \n\t"
433 " move.4 %0, CSR \n\t" /* get csr and source3
434 * implicit operands */
435 " move.4 %1, source3 \n\t"
436 " move.4 (%5), acc0_lo \n\t" /* get accumulators */
437 " move.4 4(%5), acc0_hi \n\t"
438 " move.4 8(%5), acc1_lo \n\t"
439 " move.4 12(%5), acc1_hi \n\t"
440 " move.4 16(%5), mac_rc16 \n\t"
441 " move.4 CSR, #0 \n\t" /* restore CSR */
442 " setcsr_flush 0 \n\t"
443 : "=m"(save_csr), "=m"(source3)
444 : "d"((1 << 8) | (thread << 9)),
445 "a"(save_dn), "a"(save_an), "a"(save_acc)
446 : "cc"
447 );
448
449 /*
450 * turn off thread select bits if they were on
451 */
452 BUG_ON((save_csr & 0x04100) != 0);
453 if (unlikely(save_csr & 0x04100)) {
454 /*
455 * Things are in funny state as thread select bits are on in
456 * csr. PANIC.
457 */
458 panic("In unaligned trap handler. Trap thread CSR has thread "
459 "select bits on.\n");
460 }
461
462 save_csr = save_csr & 0x1000ff;
463
464 /*
465 * get the source1 operand
466 */
467 source1 = 0;
468 if (src_operand) {
469 unsigned char *src_address;
470
471 /*
472 * source1 comes from memory
473 */
474 BUG_ON(!(format == TWO_OP || format == TWO_OP_2 ||
475 format == SRC || format == SRC_2));
476 src_address = unaligned_get_address(thread, inst & 0x7ff,
477 four_byte, save_an,
478 save_dn, &write_back_an);
479
480 /*
481 * get data (possibly unaligned)
482 */
483 if (likely(four_byte)) {
484 source_data = (*src_address << 24) |
485 (*(src_address + 1) << 16) |
486 (*(src_address + 2) << 8) |
487 *(src_address + 3);
488 source1 = source_data;
489 } else {
490 source1 = *src_address << 8 |
491 *(src_address + 1);
492
493 /*
494 * Source is not extended if the instrution is MOVE.2 or
495 * if the cpu CHIP_ID >= 0x30000 and the instruction is
496 * either LSL.2 or LSR.2. All other cases have to be
497 * sign extended.
498 */
499 if ((!(op == 2 && subop == MOVE_2)) &&
500 (!((chip_id_copy >= 0x30000) &&
501 (subop == LSL_2 || subop == LSR_2)))) {
502 /*
503 * Have to sign extend the .2 entry.
504 */
505 source1 = ((unsigned int)
506 ((signed int)
507 ((signed short) source1)));
508 }
509 }
510 } else if (likely(op != MOVEI)) {
511 /*
512 * source1 comes from a register, using move.4 d0, src1
513 * unaligned_emulate_get_source is pointer to code to insert remulated instruction
514 */
515 extern unsigned int unaligned_emulate_get_src;
516 *((int *)&unaligned_emulate_get_src) &= ~(0x7ff);
517 *((int *)&unaligned_emulate_get_src) |= (inst & 0x7ff);
518 flush_dcache_range((unsigned long)(&unaligned_emulate_get_src),
519 (unsigned long)(&unaligned_emulate_get_src) + 4);
520
521 asm volatile (
522 /* source1 uses thread's registers */
523 " move.4 CSR, %1 \n\t"
524 " setcsr_flush 0 \n\t"
525 "unaligned_emulate_get_src: \n\t"
526 " move.4 %0, #0 \n\t"
527 " setcsr #0 \n\t"
528 " setcsr_flush 0 \n\t"
529 : "=d" (source1)
530 : "d" ((1 << 8) | (thread << 9))
531 : "cc"
532 );
533 }
534
535 /*
536 * get the destination address
537 */
538 if (dest_operand) {
539 BUG_ON(!(format == TWO_OP || format == TWO_OP_2 ||
540 format == DEST || format == DEST_2));
541 dest_address = unaligned_get_address(thread,
542 ((inst >> 16) & 0x7ff),
543 four_byte, save_an,
544 save_dn, &write_back_an);
545 }
546
547 if (write_back_an) {
548 /*
549 * restore any modified An registers
550 */
551 asm volatile (
552 " move.4 CSR, %0 \n\t"
553 " setcsr_flush 0 \n\t"
554 " move.4 a0, (%1) \n\t"
555 " move.4 a1, 4(%1) \n\t"
556 " move.4 a2, 8(%1) \n\t"
557 " move.4 a3, 12(%1) \n\t"
558 " move.4 a4, 16(%1) \n\t"
559 " move.4 a5, 20(%1) \n\t"
560 " move.4 a6, 24(%1) \n\t"
561 " move.4 a7, 28(%1) \n\t"
562 " setcsr #0 \n\t"
563 " setcsr_flush 0 \n\t"
564 :
565 : "d" ((1 << 14) | (thread << 15)), "a" (save_an)
566 : "cc"
567 );
568 }
569
570 /*
571 * get source 2 register if needed, and modify inst to use d1 for
572 * source-2 source-2 will come from this thread, not the trapping thread
573 */
574 source2 = 0;
575 if ((op >= 8 && op <= 0x17) ||
576 ((op == 2 || op == 6) && (inst & 0x4000000))) {
577 int src_dn = (inst >> 11) & 0xf;
578 source2 = save_dn[src_dn];
579 /*
580 * force the emulated instruction to use d1 for source2 operand
581 */
582 emulate_inst = (emulate_inst & 0xffff07ff) | 0x800;
583 }
584
585 if (likely(op != MOVEI)) {
586 /*
587 * change emulated instruction source1 to d0
588 */
589 emulate_inst &= ~0x7ff;
590 emulate_inst |= 1 << 8;
591 }
592
593 if (unlikely(op == 6 || op == 2)) {
594 /*
595 * Set destination to d0
596 */
597 emulate_inst &= ~(0xf << 16);
598 } else if (likely(op != CMPI)) {
599 /*
600 * Set general destination field to d0.
601 */
602 emulate_inst &= ~(0x7ff << 16);
603 emulate_inst |= 1 << 24;
604 }
605
606 /*
607 * execute emulated instruction d0, to d0, no memory access
608 * source2 if needed will be in d1
609 * source3, CSR, and accumulators are set up before execution
610 */
611 *((unsigned int *)&trap_emulate) = emulate_inst;
612 flush_dcache_range((unsigned long)(&trap_emulate),
613 (unsigned long)(&trap_emulate) + 4);
614
615 result = ubicom32_emulate_insn(source1, source2, source3,
616 save_acc, &save_csr);
617
618 /*
619 * set the result value
620 */
621 if (dest_operand) {
622 /*
623 * copy result to memory
624 */
625 if (four_byte) {
626 *dest_address++ =
627 (unsigned char)((result >> 24) & 0xff);
628 *dest_address++ =
629 (unsigned char)((result >> 16) & 0xff);
630 }
631 *dest_address++ = (unsigned char)((result >> 8) & 0xff);
632 *dest_address = (unsigned char)(result & 0xff);
633 } else if (likely(op != CMPI)) {
634 /*
635 * copy result to a register, using move.4 dest, result
636 */
637 extern unsigned int unaligned_trap_set_result;
638 *((unsigned int *)&unaligned_trap_set_result) &= ~0x7ff0000;
639
640 if (op == 2 || op == 6) {
641 *((unsigned int *)&unaligned_trap_set_result) |=
642 ((inst & 0x000f0000) | 0x01000000);
643 } else {
644 *((unsigned int *)&unaligned_trap_set_result) |=
645 (inst & 0x7ff0000);
646 }
647 flush_dcache_range((unsigned long)&unaligned_trap_set_result,
648 ((unsigned long)(&unaligned_trap_set_result) + 4));
649
650 asm volatile (
651 /* result uses thread's registers */
652 " move.4 CSR, %1 \n\t"
653 " setcsr_flush 0 \n\t"
654 "unaligned_trap_set_result: \n\t"
655 " move.4 #0, %0 \n\t"
656 " setcsr #0 \n\t"
657 " setcsr_flush 0 \n\t"
658 :
659 : "d"(result), "d" ((1 << 14) | (thread << 15))
660 : "cc"
661 );
662 }
663
664 /*
665 * bump PC in thread and restore implicit register changes
666 */
667 asm volatile (
668 " move.4 CSR, %0 \n\t"
669 " setcsr_flush 0 \n\t"
670 " move.4 pc, %1 \n\t"
671 " move.4 acc0_lo, (%3) \n\t"
672 " move.4 acc0_hi, 4(%3) \n\t"
673 " move.4 acc1_lo, 8(%3) \n\t"
674 " move.4 acc1_hi, 12(%3) \n\t"
675 " move.4 mac_rc16, 16(%3) \n\t"
676 " move.4 CSR, %2 \n\t"
677 " setcsr #0 \n\t"
678 " setcsr_flush 0 \n\t"
679 :
680 : "d"((1 << 14) | (thread << 15)),
681 "d"(pc + 4), "d"(save_csr), "a"(save_acc)
682 : "cc"
683 );
684 }
685
686 /*
687 * unaligned_only()
688 * Return true if either of the unaligned causes are set (and no others).
689 */
690 int unaligned_only(unsigned int cause)
691 {
692 unsigned int unaligned_cause_mask =
693 (1 << TRAP_CAUSE_DST_MISALIGNED) |
694 (1 << TRAP_CAUSE_SRC1_MISALIGNED);
695
696 BUG_ON(cause == 0);
697 return (cause & unaligned_cause_mask) == cause;
698 }
OpenWrt Source Repository