From 93f9564224c6e5ec477722c066f6ae3bd92ca3f3 Mon Sep 17 00:00:00 2001 From: Bjorn Andersson Date: Thu, 27 Jul 2017 18:39:21 -0700 Subject: [PATCH] rmtfs: Interface qcom_rfsa device for mem access Attempt to open /dev/qcom_rfsa1 and use this instead of memory mapping /dev/mem, while falling back to the old behavior. This allow us to drop the dependency on /dev/mem access and will aid supporting multiple memory regions. Signed-off-by: Bjorn Andersson --- Makefile | 2 +- rmtfs.c | 4 +- sharedmem.c | 169 ++++++++++++++++++++++++++++++++++++++++++++-------- 3 files changed, 145 insertions(+), 30 deletions(-) diff --git a/Makefile b/Makefile index 3b7c683..403d562 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,7 @@ OUT := rmtfs CFLAGS := -Wall -g -I../qrtr/lib -O2 -LDFLAGS := -L../qrtr -lqrtr +LDFLAGS := -L../qrtr -lqrtr -ludev prefix := /usr/local SRCS := qmi_rmtfs.c qmi_tlv.c rmtfs.c sharedmem.c storage.c util.c diff --git a/rmtfs.c b/rmtfs.c index 22b9eab..558dcd7 100644 --- a/rmtfs.c +++ b/rmtfs.c @@ -496,10 +496,8 @@ int main(int argc, char **argv) dbgprintf_enabled = true; rmem = rmtfs_mem_open(); - if (!rmem) { - fprintf(stderr, "failed to initialize rmtfs shared memory\n"); + if (!rmem) return 1; - } ret = storage_open(); if (ret) { diff --git a/sharedmem.c b/sharedmem.c index a991e2f..8c42fb8 100644 --- a/sharedmem.c +++ b/sharedmem.c @@ -1,11 +1,14 @@ #include +#include #include #include #include #include +#include #include #include #include +#include #include #include #include "rmtfs.h" @@ -19,6 +22,98 @@ struct rmtfs_mem { int fd; }; +static int parse_hex_sysattr(struct udev_device *dev, const char *name, + unsigned long *value) +{ + unsigned long val; + const char *buf; + char *endptr; + + buf = udev_device_get_sysattr_value(dev, name); + if (!buf) + return -ENOENT; + + errno = 0; + val = strtoul(buf, &endptr, 16); + if ((val == LONG_MAX && errno == ERANGE) || endptr == buf) { + return -errno; + } + + *value = val; + + return 0; +} + +static int rmtfs_mem_open_rfsa(struct rmtfs_mem *rmem, int client_id) +{ + struct udev_device *dev; + struct udev *udev; + int saved_errno; + struct stat sb; + char path[32]; + int ret; + int fd; + + sprintf(path, "/dev/qcom_rfsa%d", client_id); + + fd = open(path, O_RDWR); + if (fd < 0) { + saved_errno = errno; + fprintf(stderr, "failed to open %s: %s\n", path, strerror(errno)); + return -saved_errno; + } + rmem->fd = fd; + + ret = fstat(fd, &sb); + if (ret < 0) { + saved_errno = errno; + fprintf(stderr, "failed to stat %s: %s\n", path, strerror(errno)); + close(fd); + goto err_close_fd; + } + + udev = udev_new(); + if (!udev) { + saved_errno = errno; + fprintf(stderr, "failed to create udev context\n"); + goto err_close_fd; + } + + dev = udev_device_new_from_devnum(udev, 'c', sb.st_rdev); + if (!dev) { + saved_errno = errno; + fprintf(stderr, "unable to find udev device\n"); + goto err_unref_udev; + } + + ret = parse_hex_sysattr(dev, "phys_addr", &rmem->address); + if (ret < 0) { + fprintf(stderr, "failed to parse phys_addr of %s\n", path); + saved_errno = -ret; + goto err_unref_dev; + } + + ret = parse_hex_sysattr(dev, "size", &rmem->size); + if (ret < 0) { + fprintf(stderr, "failed to parse size of %s\n", path); + saved_errno = -ret; + goto err_unref_dev; + } + + udev_device_unref(dev); + udev_unref(udev); + + return 0; + +err_unref_dev: + udev_device_unref(dev); +err_unref_udev: + udev_unref(udev); +err_close_fd: + close(fd); + return -saved_errno; +} + struct rmtfs_mem *rmtfs_mem_open(void) { struct rmtfs_mem *rmem; @@ -30,25 +125,34 @@ struct rmtfs_mem *rmtfs_mem_open(void) if (!rmem) return NULL; - ret = rmtfs_mem_enumerate(rmem); - if (ret < 0) + memset(rmem, 0, sizeof(*rmem)); + + ret = rmtfs_mem_open_rfsa(rmem, 1); + if (ret < 0 && ret != -ENOENT) { goto err; + } else if (ret < 0) { + fprintf(stderr, "falling back to /dev/mem access\n"); - fd = open("/dev/mem", O_RDWR|O_SYNC); - if (fd < 0) { - fprintf(stderr, "failed to open /dev/mem\n"); - goto err; + ret = rmtfs_mem_enumerate(rmem); + if (ret < 0) + goto err; + + fd = open("/dev/mem", O_RDWR|O_SYNC); + if (fd < 0) { + fprintf(stderr, "failed to open /dev/mem\n"); + goto err; + } + + base = mmap(0, rmem->size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, rmem->address); + if (base == MAP_FAILED) { + fprintf(stderr, "failed to mmap: %s\n", strerror(errno)); + goto err_close_fd; + } + + rmem->base = base; + rmem->fd = fd; } - base = mmap(0, rmem->size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, rmem->address); - if (base == MAP_FAILED) { - fprintf(stderr, "failed to mmap: %s\n", strerror(errno)); - goto err_close_fd; - } - - rmem->base = base; - rmem->fd = fd; - return rmem; err_close_fd: @@ -93,37 +197,50 @@ static void *rmtfs_mem_ptr(struct rmtfs_mem *rmem, unsigned phys_address, ssize_ ssize_t rmtfs_mem_read(struct rmtfs_mem *rmem, unsigned long phys_address, void *buf, ssize_t len) { + off_t offset; void *ptr; - ptr = rmtfs_mem_ptr(rmem, phys_address, len); - if (!ptr) - return -EINVAL; + if (rmem->base) { + ptr = rmtfs_mem_ptr(rmem, phys_address, len); + if (!ptr) + return -EINVAL; - memcpy(buf, ptr, len); + memcpy(buf, ptr, len); + } else { + offset = phys_address - rmem->address; + len = pread(rmem->fd, buf, len, offset); + } return len; } ssize_t rmtfs_mem_write(struct rmtfs_mem *rmem, unsigned long phys_address, const void *buf, ssize_t len) { + off_t offset; void *ptr; - ptr = rmtfs_mem_ptr(rmem, phys_address, len); - if (!ptr) - return -EINVAL; + if (rmem->base) { + ptr = rmtfs_mem_ptr(rmem, phys_address, len); + if (!ptr) + return -EINVAL; - memcpy(ptr, buf, len); + memcpy(ptr, buf, len); + } else { + offset = phys_address - rmem->address; + len = pwrite(rmem->fd, buf, len, offset); + } return len; } void rmtfs_mem_close(struct rmtfs_mem *rmem) { - munmap(rmem->base, rmem->size); + if (rmem->base) + munmap(rmem->base, rmem->size); + close(rmem->fd); - rmem->fd = -1; - rmem->base = MAP_FAILED; + free(rmem); } static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem)