11913091 |
1 | // vim:shiftwidth=2:expandtab |
2 | #include <stdio.h> |
3 | #include <string.h> |
4 | #include <sys/types.h> |
5 | #include <sys/stat.h> |
6 | #include <fcntl.h> |
7 | #include <sys/mman.h> |
3d295a9f |
8 | #include <unistd.h> |
11913091 |
9 | |
10 | #include "header.h" |
11 | #include "sys_cacheflush.h" |
12 | |
0881206b |
13 | #define strace(...) |
c1ffd5ba |
14 | //#define strace printf |
0881206b |
15 | |
11913091 |
16 | static const unsigned int sig_open[] = { |
17 | 0xe59cc000, 0xe33c0000, 0x1a000003, 0xef900005 |
18 | }; |
19 | |
20 | static const unsigned int sig_mmap[] = { |
21 | 0xe92d000f, 0xe1a0000d, 0xef90005a, 0xe28dd010 |
22 | }; |
23 | |
24 | static const unsigned int sig_mmap_[] = { |
25 | 0xe52d5004, 0xe59d5008, 0xe52d4004, 0xe59d4008, |
26 | 0xe1b0ca05, 0x1a000006, 0xe1a05625, 0xef9000c0 |
27 | }; |
28 | |
3d295a9f |
29 | static const unsigned int sig_read[] = { |
30 | 0xe59fc080, 0xe59cc000, 0xe33c0000, 0x1a000003, 0xef900003 |
31 | }; |
32 | |
c1ffd5ba |
33 | enum { |
34 | FAKEDEV_MEM = 10001, |
35 | FAKEDEV_GPIO, |
36 | FAKEDEV_FB0, |
37 | FAKEDEV_FB1, |
38 | FAKEDEV_MMUHACK, |
39 | }; |
40 | |
41 | static const struct { |
42 | const char *name; |
43 | int fd; |
44 | } takeover_devs[] = { |
45 | { "/dev/mem", FAKEDEV_MEM }, |
46 | { "/dev/GPIO", FAKEDEV_GPIO }, |
47 | { "/dev/fb0", FAKEDEV_FB0 }, |
48 | { "/dev/fb/0", FAKEDEV_FB0 }, |
49 | { "/dev/fb1", FAKEDEV_FB1 }, |
50 | { "/dev/fb/1", FAKEDEV_FB1 }, |
51 | { "/dev/mmuhack", FAKEDEV_MMUHACK }, |
52 | }; |
11913091 |
53 | |
54 | static int w_open(const char *pathname, int flags, mode_t mode) |
55 | { |
c1ffd5ba |
56 | int i, ret; |
57 | |
58 | for (i = 0; i < ARRAY_SIZE(takeover_devs); i++) { |
59 | if (strcmp(pathname, takeover_devs[i].name) == 0) { |
60 | ret = takeover_devs[i].fd; |
61 | break; |
62 | } |
63 | } |
64 | |
65 | if (i == ARRAY_SIZE(takeover_devs)) |
3d295a9f |
66 | ret = open(pathname, flags, mode); |
11913091 |
67 | |
0881206b |
68 | strace("open(%s) = %d\n", pathname, ret); |
11913091 |
69 | return ret; |
70 | } |
71 | |
72 | static void *w_mmap(void *addr, size_t length, int prot, int flags, int fd, off_t offset) |
73 | { |
74 | void *ret; |
c1ffd5ba |
75 | if (fd != FAKEDEV_MEM) |
11913091 |
76 | ret = mmap(addr, length, prot, flags, fd, offset); |
77 | else |
78 | ret = emu_mmap_dev(length, prot, flags, offset); |
79 | |
c1ffd5ba |
80 | // threads are using heap before they mmap their stack |
81 | // printf needs valid stack for pthtead/errno |
82 | if (((long)&ret & 0xf0000000) == 0xb0000000) |
83 | strace("mmap(%p, %x, %x, %x, %d, %lx) = %p\n", addr, length, prot, flags, fd, (long)offset, ret); |
11913091 |
84 | return ret; |
85 | } |
86 | #define w_mmap_ w_mmap |
87 | |
3d295a9f |
88 | ssize_t w_read(int fd, void *buf, size_t count) |
89 | { |
90 | ssize_t ret; |
c1ffd5ba |
91 | if (fd != FAKEDEV_GPIO) |
3d295a9f |
92 | return read(fd, buf, count); |
93 | |
94 | ret = emu_read_gpiodev(buf, count); |
95 | //printf("read(%d, %p, %d) = %d\n", fd, buf, count, ret); |
96 | return ret; |
97 | } |
98 | |
11913091 |
99 | #define PATCH(f) { sig_##f, ARRAY_SIZE(sig_##f), w_##f } |
100 | |
101 | static const struct { |
102 | const unsigned int *sig; |
103 | size_t sig_cnt; |
104 | void *func; |
105 | } patches[] = { |
106 | PATCH(open), |
107 | PATCH(mmap), |
108 | PATCH(mmap_), // mmap using mmap2 syscall |
3d295a9f |
109 | PATCH(read), |
11913091 |
110 | }; |
111 | |
112 | void do_patches(void *ptr, unsigned int size) |
113 | { |
114 | int i, s; |
115 | |
116 | for (i = 0; i < ARRAY_SIZE(patches); i++) { |
117 | const unsigned int *sig = patches[i].sig; |
118 | unsigned int *seg = (void *)(((long)ptr + 3) & ~3); |
119 | unsigned int *seg_end = seg + size / 4; |
120 | unsigned int sig0 = sig[0]; |
121 | |
122 | for (; seg < seg_end; seg++) { |
123 | if (*seg != sig0) |
124 | continue; |
125 | |
126 | for (s = 1; s < patches[i].sig_cnt; s++) |
127 | if (seg[s] != sig[s]) |
128 | break; |
129 | |
130 | if (s == patches[i].sig_cnt) |
131 | goto found; |
132 | } |
133 | continue; |
134 | |
135 | found: |
136 | printf(" patch #%i @ %08x\n", i, (int)seg); |
137 | seg[0] = 0xe59ff000; // ldr pc, [pc] |
138 | seg[1] = 0; |
139 | seg[2] = (unsigned int)patches[i].func; |
140 | } |
141 | |
142 | sys_cacheflush(ptr, (char *)ptr + size); |
143 | } |
144 | |