1 #include <sys/mman.h>
2 #include <sys/stat.h>
3 #include <sys/types.h>
4 #include <dirent.h>
5 #include <errno.h>
6 #include <fcntl.h>
7 #ifndef ANDROID
8 #include <libudev.h>
9 #else
10 #include <sys/endian.h>
11 #endif
12 #include <stdbool.h>
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <limits.h>
16 #include <string.h>
17 #include <unistd.h>
18 #include "rmtfs.h"
19
20 static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem);
21
22 struct rmtfs_mem {
23 uint64_t address;
24 uint64_t size;
25 void *base;
26 int fd;
27 };
28
29 #ifndef ANDROID
30
parse_hex_sysattr(struct udev_device * dev,const char * name,uint64_t * value)31 static int parse_hex_sysattr(struct udev_device *dev, const char *name,
32 uint64_t *value)
33 {
34 unsigned long long val;
35 const char *buf;
36 char *endptr;
37
38 buf = udev_device_get_sysattr_value(dev, name);
39 if (!buf)
40 return -ENOENT;
41
42 errno = 0;
43 val = strtoull(buf, &endptr, 16);
44 if ((val == ULLONG_MAX && errno == ERANGE) || endptr == buf) {
45 return -errno;
46 }
47
48 *value = val;
49
50 return 0;
51 }
52
rmtfs_mem_open_rfsa(struct rmtfs_mem * rmem,int client_id)53 static int rmtfs_mem_open_rfsa(struct rmtfs_mem *rmem, int client_id)
54 {
55 struct udev_device *dev;
56 struct udev *udev;
57 int saved_errno;
58 struct stat sb;
59 char path[32];
60 int ret;
61 int fd;
62
63 sprintf(path, "/dev/qcom_rmtfs_mem%d", client_id);
64
65 fd = open(path, O_RDWR);
66 if (fd < 0) {
67 saved_errno = errno;
68 fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
69 return -saved_errno;
70 }
71 rmem->fd = fd;
72
73 ret = fstat(fd, &sb);
74 if (ret < 0) {
75 saved_errno = errno;
76 fprintf(stderr, "failed to stat %s: %s\n", path, strerror(errno));
77 close(fd);
78 goto err_close_fd;
79 }
80
81 udev = udev_new();
82 if (!udev) {
83 saved_errno = errno;
84 fprintf(stderr, "failed to create udev context\n");
85 goto err_close_fd;
86 }
87
88 dev = udev_device_new_from_devnum(udev, 'c', sb.st_rdev);
89 if (!dev) {
90 saved_errno = errno;
91 fprintf(stderr, "unable to find udev device\n");
92 goto err_unref_udev;
93 }
94
95 ret = parse_hex_sysattr(dev, "phys_addr", &rmem->address);
96 if (ret < 0) {
97 fprintf(stderr, "failed to parse phys_addr of %s\n", path);
98 saved_errno = -ret;
99 goto err_unref_dev;
100 }
101
102 ret = parse_hex_sysattr(dev, "size", &rmem->size);
103 if (ret < 0) {
104 fprintf(stderr, "failed to parse size of %s\n", path);
105 saved_errno = -ret;
106 goto err_unref_dev;
107 }
108
109 udev_device_unref(dev);
110 udev_unref(udev);
111
112 return 0;
113
114 err_unref_dev:
115 udev_device_unref(dev);
116 err_unref_udev:
117 udev_unref(udev);
118 err_close_fd:
119 close(fd);
120 return -saved_errno;
121 }
122
rmtfs_mem_open_uio(struct rmtfs_mem * rmem,int client_id)123 static int rmtfs_mem_open_uio(struct rmtfs_mem *rmem, int client_id)
124 {
125 struct udev_device *dev;
126 struct udev *udev;
127 int saved_errno;
128 struct stat sb;
129 char path[32];
130 int ret;
131 int fd;
132
133 snprintf(path, sizeof(path), "/dev/qcom_rmtfs_uio%d", client_id);
134
135 fd = open(path, O_RDWR);
136 if (fd < 0) {
137 saved_errno = errno;
138 fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
139 return -saved_errno;
140 }
141 rmem->fd = fd;
142
143 ret = fstat(fd, &sb);
144 if (ret < 0) {
145 saved_errno = errno;
146 fprintf(stderr, "failed to stat %s: %s\n", path, strerror(errno));
147 close(fd);
148 goto err_close_fd;
149 }
150
151 udev = udev_new();
152 if (!udev) {
153 saved_errno = errno;
154 fprintf(stderr, "failed to create udev context\n");
155 goto err_close_fd;
156 }
157
158 dev = udev_device_new_from_devnum(udev, 'c', sb.st_rdev);
159 if (!dev) {
160 saved_errno = errno;
161 fprintf(stderr, "unable to find udev device\n");
162 goto err_unref_udev;
163 }
164
165 ret = parse_hex_sysattr(dev, "maps/map0/addr", &rmem->address);
166 if (ret < 0) {
167 fprintf(stderr, "failed to parse phys_addr of %s\n", path);
168 saved_errno = -ret;
169 goto err_unref_dev;
170 }
171
172 ret = parse_hex_sysattr(dev, "maps/map0/size", &rmem->size);
173 if (ret < 0) {
174 fprintf(stderr, "failed to parse size of %s\n", path);
175 saved_errno = -ret;
176 goto err_unref_dev;
177 }
178
179 rmem->base = mmap(0, rmem->size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);
180 if (rmem->base == MAP_FAILED) {
181 saved_errno = errno;
182 fprintf(stderr, "failed to mmap: %s\n", strerror(errno));
183 goto err_unref_dev;
184 }
185
186 udev_device_unref(dev);
187 udev_unref(udev);
188
189 return 0;
190
191 err_unref_dev:
192 udev_device_unref(dev);
193 err_unref_udev:
194 udev_unref(udev);
195 err_close_fd:
196 close(fd);
197 return -saved_errno;
198 }
199
200 #else
201
202 #define PAGE_SIZE 4096
203
rmtfs_mem_open_rfsa(struct rmtfs_mem * rmem,int client_id)204 static int rmtfs_mem_open_rfsa(struct rmtfs_mem *rmem, int client_id)
205 {
206 int saved_errno;
207 int fd;
208 char path[PATH_MAX];
209 char val[PAGE_SIZE];
210 char *endptr;
211
212 errno = 0;
213
214 snprintf(path, sizeof(path), "/dev/qcom_rmtfs_mem%d", client_id);
215 rmem->fd = open(path, O_RDWR);
216 if (rmem->fd < 0) {
217 saved_errno = errno;
218 fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
219 return -saved_errno;
220 }
221
222 snprintf(path, sizeof(path), "/sys/class/rmtfs/qcom_rmtfs_mem%d/phys_addr", client_id);
223 fd = open(path, O_RDONLY);
224 if (fd < 0) {
225 saved_errno = errno;
226 fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
227 return -saved_errno;
228 }
229 read(fd, val, sizeof(val));
230 rmem->address = strtoull(val, &endptr, 16);
231 if ((rmem->address == ULLONG_MAX && errno == ERANGE) || endptr == val) {
232 saved_errno = errno;
233 goto err_close_fd;
234 }
235 close(fd);
236
237 snprintf(path, sizeof(path), "/sys/class/rmtfs/qcom_rmtfs_mem%d/size", client_id);
238 fd = open(path, O_RDONLY);
239 if (fd < 0) {
240 saved_errno = errno;
241 fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno));
242 return -saved_errno;
243 }
244 read(fd, val, sizeof(val));
245 rmem->size = strtoull(val, &endptr, 16);
246 if ((rmem->size == ULLONG_MAX && errno == ERANGE) || endptr == val) {
247 saved_errno = errno;
248 goto err_close_fd;
249 }
250 close(fd);
251
252 return 0;
253
254 err_close_fd:
255 close(fd);
256 return -saved_errno;
257 }
258
rmtfs_mem_open_uio(struct rmtfs_mem * rmem __unused,int client_id __unused)259 static int rmtfs_mem_open_uio(struct rmtfs_mem *rmem __unused, int client_id __unused)
260 {
261 fprintf(stderr, "uio access is not supported on ANDROID yet\n");
262 return -EINVAL;
263 }
264
265 #endif
266
rmtfs_mem_open(void)267 struct rmtfs_mem *rmtfs_mem_open(void)
268 {
269 struct rmtfs_mem *rmem;
270 void *base;
271 int ret;
272 int fd;
273
274 rmem = malloc(sizeof(*rmem));
275 if (!rmem)
276 return NULL;
277
278 memset(rmem, 0, sizeof(*rmem));
279
280 ret = rmtfs_mem_open_rfsa(rmem, 1);
281 if (ret < 0 && ret != -ENOENT) {
282 goto err;
283 } else if (ret < 0) {
284 fprintf(stderr, "falling back to uio access\n");
285 ret = rmtfs_mem_open_uio(rmem, 1);
286 if (ret < 0 && ret != -ENOENT) {
287 goto err;
288 } else if (ret < 0) {
289 fprintf(stderr, "falling back to /dev/mem access\n");
290
291 ret = rmtfs_mem_enumerate(rmem);
292 if (ret < 0)
293 goto err;
294
295 fd = open("/dev/mem", O_RDWR|O_SYNC);
296 if (fd < 0) {
297 fprintf(stderr, "failed to open /dev/mem\n");
298 goto err;
299 }
300
301 base = mmap(0, rmem->size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, rmem->address);
302 if (base == MAP_FAILED) {
303 fprintf(stderr, "failed to mmap: %s\n", strerror(errno));
304 goto err_close_fd;
305 }
306
307 rmem->base = base;
308 rmem->fd = fd;
309 }
310 }
311
312 return rmem;
313
314 err_close_fd:
315 close(fd);
316 err:
317 free(rmem);
318 return NULL;
319 }
320
rmtfs_mem_alloc(struct rmtfs_mem * rmem,size_t alloc_size)321 int64_t rmtfs_mem_alloc(struct rmtfs_mem *rmem, size_t alloc_size)
322 {
323 if (alloc_size > rmem->size) {
324 fprintf(stderr,
325 "[RMTFS] rmtfs shared memory not large enough for allocation request 0x%zx vs 0x%lx\n",
326 alloc_size, rmem->size);
327 return -EINVAL;
328 }
329
330 return rmem->address;
331 }
332
rmtfs_mem_free(struct rmtfs_mem * rmem __unused)333 void rmtfs_mem_free(struct rmtfs_mem *rmem __unused)
334 {
335 }
336
rmtfs_mem_ptr(struct rmtfs_mem * rmem,unsigned long phys_address,ssize_t len)337 static void *rmtfs_mem_ptr(struct rmtfs_mem *rmem, unsigned long phys_address, ssize_t len)
338 {
339 uint64_t start;
340 uint64_t end;
341
342 if (len < 0)
343 return NULL;
344
345 start = phys_address;
346 end = start + len;
347
348 if (start < rmem->address || end > rmem->address + rmem->size)
349 return NULL;
350
351 return (char*)rmem->base + phys_address - rmem->address;
352 }
353
rmtfs_mem_read(struct rmtfs_mem * rmem,unsigned long phys_address,void * buf,ssize_t len)354 ssize_t rmtfs_mem_read(struct rmtfs_mem *rmem, unsigned long phys_address, void *buf, ssize_t len)
355 {
356 off_t offset;
357 void *ptr;
358
359 if (rmem->base) {
360 ptr = rmtfs_mem_ptr(rmem, phys_address, len);
361 if (!ptr)
362 return -EINVAL;
363
364 memcpy(buf, ptr, len);
365 } else {
366 offset = phys_address - rmem->address;
367 len = pread(rmem->fd, buf, len, offset);
368 }
369
370 return len;
371 }
372
rmtfs_mem_write(struct rmtfs_mem * rmem,unsigned long phys_address,const void * buf,ssize_t len)373 ssize_t rmtfs_mem_write(struct rmtfs_mem *rmem, unsigned long phys_address, const void *buf, ssize_t len)
374 {
375 off_t offset;
376 void *ptr;
377
378 if (rmem->base) {
379 ptr = rmtfs_mem_ptr(rmem, phys_address, len);
380 if (!ptr)
381 return -EINVAL;
382
383 memcpy(ptr, buf, len);
384 } else {
385 offset = phys_address - rmem->address;
386 len = pwrite(rmem->fd, buf, len, offset);
387 }
388
389 return len;
390 }
391
rmtfs_mem_close(struct rmtfs_mem * rmem)392 void rmtfs_mem_close(struct rmtfs_mem *rmem)
393 {
394 if (rmem->base)
395 munmap(rmem->base, rmem->size);
396
397 close(rmem->fd);
398
399 free(rmem);
400 }
401
rmtfs_mem_enumerate(struct rmtfs_mem * rmem)402 static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem)
403 {
404 union {
405 uint32_t dw[2];
406 uint64_t qw[2];
407 } reg;
408 struct dirent *de;
409 int basefd;
410 int dirfd;
411 int regfd;
412 DIR *dir;
413 int ret = 0;
414 int n;
415
416 basefd = open("/proc/device-tree/reserved-memory/", O_DIRECTORY);
417 dir = fdopendir(basefd);
418 if (!dir) {
419 fprintf(stderr,
420 "Unable to open reserved-memory device tree node: %s\n",
421 strerror(-errno));
422 close(basefd);
423 return -1;
424 }
425
426 while ((de = readdir(dir)) != NULL) {
427 if (strncmp(de->d_name, "rmtfs", 5) != 0)
428 continue;
429
430 dirfd = openat(basefd, de->d_name, O_DIRECTORY);
431 if (dirfd < 0) {
432 fprintf(stderr, "failed to open %s: %s\n",
433 de->d_name, strerror(-errno));
434 ret = -1;
435 goto out;
436 }
437
438 regfd = openat(dirfd, "reg", O_RDONLY);
439 if (regfd < 0) {
440 fprintf(stderr, "failed to open reg of %s: %s\n",
441 de->d_name, strerror(-errno));
442 ret = -1;
443 goto out;
444 }
445
446 n = read(regfd, ®, sizeof(reg));
447 if (n == 2 * sizeof(uint32_t)) {
448 rmem->address = be32toh(reg.dw[0]);
449 rmem->size = be32toh(reg.dw[1]);
450 } else if (n == 2 * sizeof(uint64_t)) {
451 rmem->address = be64toh(reg.qw[0]);
452 rmem->size = be64toh(reg.qw[1]);
453 } else {
454 fprintf(stderr, "failed to read reg of %s: %s\n",
455 de->d_name, strerror(-errno));
456 ret = -1;
457 }
458
459 close(regfd);
460 close(dirfd);
461 break;
462 }
463
464 out:
465 closedir(dir);
466 close(basefd);
467 return ret;
468 }
469