1 | /* |
---|
2 | * Address Probing for M68k/ColdFire |
---|
3 | */ |
---|
4 | |
---|
5 | #include <bsp.h> |
---|
6 | #include <string.h> |
---|
7 | #include <rtems/m68k/sim.h> |
---|
8 | |
---|
9 | #if (M68K_COLDFIRE_ARCH == 1) |
---|
10 | # define EXCEPTION_FRAME_PC_OFFSET "4" |
---|
11 | #else |
---|
12 | # define EXCEPTION_FRAME_PC_OFFSET "2" |
---|
13 | #endif |
---|
14 | |
---|
15 | typedef int (*MemProber)(void *from, void *to); |
---|
16 | int memProbeByte(void *from, void *to); |
---|
17 | int memProbeShort(void *from, void *to); |
---|
18 | int memProbeLong(void *from, void *to); |
---|
19 | int memProbeCatcher(void); |
---|
20 | rtems_status_code bspExtMemProbe(void *addr, int write, int size, void *pval); |
---|
21 | |
---|
22 | __asm__( |
---|
23 | ".text\n" |
---|
24 | "memProbeByte: \n" |
---|
25 | " move.l %sp@(4),%a0\n" |
---|
26 | " move.b %a0@,%d0 \n" |
---|
27 | " move.l %sp@(8),%a0\n" |
---|
28 | " move.b %d0,%a0@ \n" |
---|
29 | " bra.b 1f \n" |
---|
30 | "memProbeShort: \n" |
---|
31 | " move.l %sp@(4),%a0\n" |
---|
32 | " move.w %a0@,%d0 \n" |
---|
33 | " move.l %sp@(8),%a0\n" |
---|
34 | " move.w %d0,%a0@ \n" |
---|
35 | " bra.b 1f \n" |
---|
36 | "memProbeLong: \n" |
---|
37 | " move.l %sp@(4),%a0\n" |
---|
38 | " move.l %a0@,%d0 \n" |
---|
39 | " move.l %sp@(8),%a0\n" |
---|
40 | " move.l %d0,%a0@ \n" |
---|
41 | "1: nop \n" |
---|
42 | " moveq.l #1,%d0 \n" |
---|
43 | " rts \n" |
---|
44 | "memProbeCatcher: \n" |
---|
45 | " move.l #1f,%d0 \n" |
---|
46 | " move.l %d0,%sp@(" EXCEPTION_FRAME_PC_OFFSET ")\n" |
---|
47 | " rte \n" |
---|
48 | "1: clr.l %d0 \n" |
---|
49 | " rts \n" |
---|
50 | ); |
---|
51 | |
---|
52 | rtems_status_code |
---|
53 | bspExtMemProbe(void *addr, int write, int size, void *pval) |
---|
54 | { |
---|
55 | rtems_status_code rval=RTEMS_SUCCESSFUL; |
---|
56 | rtems_interrupt_level level; |
---|
57 | unsigned long buf; |
---|
58 | MemProber probe; |
---|
59 | void *saveVector; |
---|
60 | void **exceptionPointer; |
---|
61 | void *vbr; |
---|
62 | |
---|
63 | /* |
---|
64 | * Sanity check |
---|
65 | */ |
---|
66 | switch (size) { |
---|
67 | case sizeof(char): probe=memProbeByte; break; |
---|
68 | case sizeof(short): probe=memProbeShort; break; |
---|
69 | case sizeof(long): probe=memProbeLong; break; |
---|
70 | default: return RTEMS_INVALID_SIZE; |
---|
71 | } |
---|
72 | |
---|
73 | /* |
---|
74 | * use a buffer to make sure we don't end up probing 'pval'. |
---|
75 | */ |
---|
76 | if (write && pval) |
---|
77 | memcpy(&buf, pval, size); |
---|
78 | |
---|
79 | /* |
---|
80 | * Get location of access fault exception |
---|
81 | */ |
---|
82 | m68k_get_vbr(vbr); |
---|
83 | exceptionPointer = (void **)((char *)vbr + (2 * 4)); |
---|
84 | |
---|
85 | /* |
---|
86 | * Probe! |
---|
87 | */ |
---|
88 | rtems_interrupt_disable(level); |
---|
89 | saveVector = *exceptionPointer; |
---|
90 | *exceptionPointer = memProbeCatcher; |
---|
91 | if (write) { |
---|
92 | if (probe(&buf, addr) == 0) |
---|
93 | rval = RTEMS_INVALID_ADDRESS; |
---|
94 | } |
---|
95 | else { |
---|
96 | if (probe(addr, &buf) == 0) |
---|
97 | rval = RTEMS_INVALID_ADDRESS; |
---|
98 | } |
---|
99 | *exceptionPointer = saveVector; |
---|
100 | rtems_interrupt_enable(level); |
---|
101 | |
---|
102 | if (!write && pval && (rval == RTEMS_SUCCESSFUL)) |
---|
103 | memcpy(pval, &buf, size); |
---|
104 | return rval; |
---|
105 | } |
---|