diff options
Diffstat (limited to 'bsps/m68k/shared/memProbe.c')
-rw-r--r-- | bsps/m68k/shared/memProbe.c | 105 |
1 files changed, 105 insertions, 0 deletions
diff --git a/bsps/m68k/shared/memProbe.c b/bsps/m68k/shared/memProbe.c new file mode 100644 index 0000000000..d352f76b6e --- /dev/null +++ b/bsps/m68k/shared/memProbe.c @@ -0,0 +1,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; +} |