source: rtems/bsps/m68k/shared/memProbe.c

Last change on this file was fc2ec62, checked in by Sebastian Huber <sebastian.huber@…>, on Mar 26, 2018 at 10:10:15 AM

bsps/m68k: Move libcpu content to bsps

This patch is a part of the BSP source reorganization.

Update #3285.

  • Property mode set to 100644
File size: 2.6 KB
Line 
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
15typedef int (*MemProber)(void *from, void *to);
16int memProbeByte(void *from, void *to);
17int memProbeShort(void *from, void *to);
18int memProbeLong(void *from, void *to);
19int memProbeCatcher(void);
20rtems_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
52rtems_status_code
53bspExtMemProbe(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}
Note: See TracBrowser for help on using the repository browser.