m68k/shared/memProbe.c

0001 /*
0002  * Address Probing for M68k/ColdFire
0003  */
0004
0005 #include <bsp.h>
0006 #include <string.h>
0007 #include <rtems/m68k/sim.h>
0008
0009 #if (M68K_COLDFIRE_ARCH == 1)
0010 # define EXCEPTION_FRAME_PC_OFFSET "4"
0011 #else
0012 # define EXCEPTION_FRAME_PC_OFFSET "2"
0013 #endif
0014
0015 typedef int (*MemProber)(void *from, void *to);
0016 int memProbeByte(void *from, void *to);
0017 int memProbeShort(void *from, void *to);
0018 int memProbeLong(void *from, void *to);
0019 int memProbeCatcher(void);
0020 rtems_status_code bspExtMemProbe(void *addr, int write, int size, void *pval);
0021
0022 __asm__(
0023     ".text\n"
0024     "memProbeByte:        \n"
0025     "   move.l %sp@(4),%a0\n"
0026     "   move.b %a0@,%d0   \n"
0027     "   move.l %sp@(8),%a0\n"
0028     "   move.b %d0,%a0@   \n"
0029     "   bra.b 1f          \n"
0030     "memProbeShort:       \n"
0031     "   move.l %sp@(4),%a0\n"
0032     "   move.w %a0@,%d0   \n"
0033     "   move.l %sp@(8),%a0\n"
0034     "   move.w %d0,%a0@   \n"
0035     "   bra.b 1f          \n"
0036     "memProbeLong:        \n"
0037     "   move.l %sp@(4),%a0\n"
0038     "   move.l %a0@,%d0   \n"
0039     "   move.l %sp@(8),%a0\n"
0040     "   move.l %d0,%a0@   \n"
0041     "1: nop               \n"
0042     "   moveq.l #1,%d0    \n"
0043     "   rts               \n"
0044     "memProbeCatcher:     \n"
0045     "   move.l #1f,%d0    \n"
0046     "   move.l %d0,%sp@(" EXCEPTION_FRAME_PC_OFFSET ")\n"
0047     "   rte               \n"
0048     "1: clr.l  %d0        \n"
0049     "   rts               \n"
0050 );
0051
0052 rtems_status_code
0053 bspExtMemProbe(void *addr, int write, int size, void *pval)
0054 {
0055     rtems_status_code rval=RTEMS_SUCCESSFUL;
0056     rtems_interrupt_level level;
0057     unsigned long buf;
0058     MemProber probe;
0059     void *saveVector;
0060     void **exceptionPointer;
0061     void *vbr;
0062
0063     /*
0064      * Sanity check
0065      */
0066     switch (size) {
0067         case sizeof(char):  probe=memProbeByte; break;
0068         case sizeof(short): probe=memProbeShort; break;
0069         case sizeof(long):  probe=memProbeLong; break;
0070         default: return RTEMS_INVALID_SIZE;
0071     }
0072
0073     /*
0074      * use a buffer to make sure we don't end up probing 'pval'.
0075      */
0076     if (write && pval)
0077         memcpy(&buf, pval, size);
0078
0079     /*
0080      * Get location of access fault exception
0081      */
0082     m68k_get_vbr(vbr);
0083     exceptionPointer = (void **)((char *)vbr + (2 * 4));
0084
0085     /*
0086      * Probe!
0087      */
0088     rtems_interrupt_disable(level);
0089     saveVector = *exceptionPointer;
0090     *exceptionPointer = memProbeCatcher;
0091     if (write) {
0092         if (probe(&buf, addr) == 0)
0093             rval = RTEMS_INVALID_ADDRESS;
0094     }
0095     else {
0096         if (probe(addr, &buf) == 0)
0097             rval = RTEMS_INVALID_ADDRESS;
0098     }
0099     *exceptionPointer = saveVector;
0100     rtems_interrupt_enable(level);
0101
0102     if (!write && pval && (rval == RTEMS_SUCCESSFUL))
0103         memcpy(pval, &buf, size);
0104     return rval;
0105 }