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