[359e537] | 1 | /* |
---|
[f8ac4fc7] | 2 | * Address Probing for M68k/ColdFire |
---|
| 3 | */ |
---|
| 4 | |
---|
| 5 | #include <bsp.h> |
---|
| 6 | #include <string.h> |
---|
[6ddc4da] | 7 | #include <rtems/m68k/sim.h> |
---|
[f8ac4fc7] | 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); |
---|
[6ddc4da] | 20 | rtems_status_code bspExtMemProbe(void *addr, int write, int size, void *pval); |
---|
[f8ac4fc7] | 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 | } |
---|