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 <bjorn.andersson@linaro.org>

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

rmtfs.c

@@ -25,7 +25,7 @@
 #define RMTFS_QMI_VERSION	1
 #define RMTFS_QMI_INSTANCE	0
 
-#define SECTOR_SIZE		512
+static struct rmtfs_mem *rmem;
 
 /* TODO: include from kernel once it lands */
 struct sockaddr_qrtr {
@@ -131,7 +131,7 @@ static void rmtfs_close(int sock, unsigned node, unsigned port, void *msg, size_
 	if (ret < 0)
 		qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL);
 
-	rmtfs_mem_free();
+	rmtfs_mem_free(rmem);
 
 respond:
 	dbgprintf("[RMTFS] close %d => (%d:%d)\n", caller_id, result.result, result.error);
@@ -157,6 +157,7 @@ static void rmtfs_iovec(int sock, unsigned node, unsigned port, void *msg, size_
 	struct rmtfs_qmi_result result = {};
 	struct rmtfs_iovec_resp *resp;
 	struct rmtfs_iovec_req *req;
+	unsigned long phys_offset;
 	uint32_t caller_id;
 	size_t num_entries;
 	uint8_t is_write;
@@ -165,6 +166,7 @@ static void rmtfs_iovec(int sock, unsigned node, unsigned port, void *msg, size_
 	ssize_t n;
 	size_t len;
 	void *ptr;
+	char buf[SECTOR_SIZE];
 	int ret;
 	int fd;
 	int i;
@@ -208,11 +210,7 @@ static void rmtfs_iovec(int sock, unsigned node, unsigned port, void *msg, size_
 	}
 
 	for (i = 0; i < num_entries; i++) {
-		ptr = rmtfs_mem_ptr(entries[i].phys_offset, entries[i].num_sector * SECTOR_SIZE);
-		if (!ptr) {
-			qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL);
-			goto respond;
-		}
+		phys_offset = entries[i].phys_offset;
 
 		n = lseek(fd, entries[i].sector_addr * SECTOR_SIZE, SEEK_SET);
 		if (n < 0) {
@@ -222,10 +220,13 @@ static void rmtfs_iovec(int sock, unsigned node, unsigned port, void *msg, size_
 		}
 
 		for (j = 0; j < entries[i].num_sector; j++) {
-			if (is_write)
-				n = write(fd, ptr, SECTOR_SIZE);
-			else
-				n = read(fd, ptr, SECTOR_SIZE);
+			if (is_write) {
+				n = rmtfs_mem_read(rmem, phys_offset, buf, SECTOR_SIZE);
+				n = write(fd, buf, n);
+			} else {
+				n = read(fd, buf, SECTOR_SIZE);
+				n = rmtfs_mem_write(rmem, phys_offset, buf, n);
+			}
 
 			if (n != SECTOR_SIZE) {
 				fprintf(stderr, "[RMTFS] failed to %s sector %d\n",
@@ -234,7 +235,7 @@ static void rmtfs_iovec(int sock, unsigned node, unsigned port, void *msg, size_
 				goto respond;
 			}
 
-			ptr += SECTOR_SIZE;
+			phys_offset += SECTOR_SIZE;
 		}
 	}
 
@@ -294,7 +295,7 @@ static void rmtfs_alloc_buf(int sock, unsigned node, unsigned port, void *msg, s
 		goto respond;
 	}
 
-	address = rmtfs_mem_alloc(alloc_size);
+	address = rmtfs_mem_alloc(rmem, alloc_size);
 	if (address < 0)
 		qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL);
 
@@ -374,16 +375,38 @@ static int handle_rfsa(int sock)
 	sl = sizeof(sq);
 	ret = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
 	if (ret < 0) {
-		fprintf(stderr, "[RMTFS] recvfrom failed: %d\n", ret);
+		ret = -errno;
+		if (ret != -ENETRESET)
+			fprintf(stderr, "[RFSA] recvfrom failed: %d\n", ret);
 		return ret;
 	}
 
-	printf("[RFSA] packet; from: %d:%d\n", sq.sq_node, sq.sq_port);
-	print_hex_dump("[RFSA <-]", buf, ret);
+	dbgprintf("[RFSA] packet; from: %d:%d\n", sq.sq_node, sq.sq_port);
+	if (dbgprintf_enabled)
+		print_hex_dump("[RFSA <-]", buf, ret);
 
 	return 0;
 }
 
+static int rmtfs_bye(uint32_t node, void *data)
+{
+	dbgprintf("[RMTFS] bye from %d\n", node);
+
+	return 0;
+}
+
+static int rmtfs_del_client(uint32_t node, uint32_t port, void *data)
+{
+	dbgprintf("[RMTFS] del_client %d:%d\n", node, port);
+
+	return 0;
+}
+
+struct qrtr_ind_ops rmtfs_ctrl_ops = {
+	.bye = rmtfs_bye,
+	.del_client = rmtfs_del_client,
+};
+
 static int handle_rmtfs(int sock)
 {
 	struct sockaddr_qrtr sq;
@@ -395,12 +418,19 @@ static int handle_rmtfs(int sock)
 	sl = sizeof(sq);
 	ret = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
 	if (ret < 0) {
-		fprintf(stderr, "[RMTFS] recvfrom failed: %d\n", ret);
+		ret = -errno;
+		if (ret != -ENETRESET)
+			fprintf(stderr, "[RMTFS] recvfrom failed: %d\n", ret);
 		return ret;
 	}
 
 	dbgprintf("[RMTFS] packet; from: %d:%d\n", sq.sq_node, sq.sq_port);
 
+	if (qrtr_is_ctrl_addr(&sq)) {
+		return qrtr_handle_ctrl_msg(&sq, buf, sizeof(buf),
+					    &rmtfs_ctrl_ops, NULL);
+	}
+
 	qmi = (struct qmi_packet*)buf;
 	if (qmi->msg_len != ret - sizeof(struct qmi_packet)) {
 		fprintf(stderr, "[RMTFS] Invalid length of incoming qmi request\n");
@@ -432,8 +462,30 @@ static int handle_rmtfs(int sock)
 	return ret;
 }
 
+static int register_services(int rfsa_fd, int rmtfs_fd)
+{
+	int ret;
+
+	ret = qrtr_publish(rfsa_fd, RFSA_QMI_SERVICE, RFSA_QMI_VERSION, RFSA_QMI_INSTANCE);
+	if (ret < 0) {
+		fprintf(stderr, "failed to publish rfsa service");
+		return ret;
+	}
+
+	ret = qrtr_publish(rmtfs_fd, RMTFS_QMI_SERVICE, RMTFS_QMI_VERSION, RMTFS_QMI_INSTANCE);
+	if (ret < 0) {
+		fprintf(stderr, "failed to publish misc ta service");
+
+		qrtr_bye(rfsa_fd, RFSA_QMI_SERVICE, RFSA_QMI_VERSION, RFSA_QMI_INSTANCE);
+		return ret;
+	}
+
+	return 0;
+}
+
 int main(int argc, char **argv)
 {
+	bool do_register = true;
 	int rmtfs_fd;
 	int rfsa_fd;
 	fd_set rfds;
@@ -443,43 +495,38 @@ int main(int argc, char **argv)
 	if (argc == 2 && strcmp(argv[1], "-v") == 0)
 		dbgprintf_enabled = true;
 
-	ret = rmtfs_mem_open();
-	if (ret) {
-		fprintf(stderr, "failed to initialize rmtfs shared memory");
+	rmem = rmtfs_mem_open();
+	if (!rmem)
 		return 1;
-	}
 
 	ret = storage_open();
 	if (ret) {
-		fprintf(stderr, "failed to initialize storage system");
+		fprintf(stderr, "failed to initialize storage system\n");
 		goto close_rmtfs_mem;
 	}
 
-	rfsa_fd = qrtr_open(0);
+	rfsa_fd = qrtr_open(RFSA_QMI_SERVICE);
 	if (rfsa_fd < 0) {
-		fprintf(stderr, "failed to create qrtr socket");
+		fprintf(stderr, "failed to create qrtr socket\n");
 		goto close_storage;
 	}
 
-	rmtfs_fd = qrtr_open(0);
+	rmtfs_fd = qrtr_open(RMTFS_QMI_SERVICE);
 	if (rmtfs_fd < 0) {
-		fprintf(stderr, "failed to create qrtr socket");
+		fprintf(stderr, "failed to create qrtr socket\n");
 		goto close_storage;
 	}
 
-	ret = qrtr_publish(rfsa_fd, RFSA_QMI_SERVICE, RFSA_QMI_VERSION, RFSA_QMI_INSTANCE);
-	if (ret < 0) {
-		fprintf(stderr, "failed to publish rfsa service");
-		goto close_storage;
-	}
-
-	ret = qrtr_publish(rmtfs_fd, RMTFS_QMI_SERVICE, RMTFS_QMI_VERSION, RMTFS_QMI_INSTANCE);
-	if (ret < 0) {
-		fprintf(stderr, "failed to publish misc ta service");
-		goto unpublish_rfsa;
-	}
-
 	for (;;) {
+		if (do_register) {
+			dbgprintf("registering services\n");
+			ret = register_services(rfsa_fd, rmtfs_fd);
+			if (ret)
+				break;
+
+			do_register = false;
+		}
+
 		FD_ZERO(&rfds);
 		FD_SET(rfsa_fd, &rfds);
 		FD_SET(rmtfs_fd, &rfds);
@@ -494,9 +541,12 @@ int main(int argc, char **argv)
 		}
 
 		if (FD_ISSET(rfsa_fd, &rfds))
-			handle_rfsa(rfsa_fd);
+			ret = handle_rfsa(rfsa_fd);
 		else if (FD_ISSET(rmtfs_fd, &rfds))
-			handle_rmtfs(rmtfs_fd);
+			ret = handle_rmtfs(rmtfs_fd);
+
+		if (ret == -ENETRESET)
+			do_register = true;
 	}
 
 	qrtr_bye(rmtfs_fd, RMTFS_QMI_SERVICE, RMTFS_QMI_VERSION, RMTFS_QMI_INSTANCE);
@@ -505,7 +555,7 @@ unpublish_rfsa:
 close_storage:
 	storage_close();
 close_rmtfs_mem:
-	rmtfs_mem_close();
+	rmtfs_mem_close(rmem);
 
 	return 0;
 }

rmtfs.h

@@ -4,6 +4,8 @@
 #include <stdint.h>
 #include "qmi_rmtfs.h"
 
+#define SECTOR_SIZE		512
+
 struct qmi_packet {
 	uint8_t flags;
 	uint16_t txn_id;
@@ -12,11 +14,14 @@ struct qmi_packet {
 	uint8_t data[];
 } __attribute__((__packed__));
 
-int rmtfs_mem_open(void);
-void rmtfs_mem_close(void);
-int64_t rmtfs_mem_alloc(size_t size);
-void *rmtfs_mem_ptr(unsigned phys_address, size_t len);
-void rmtfs_mem_free(void);
+struct rmtfs_mem;
+
+struct rmtfs_mem *rmtfs_mem_open(void);
+void rmtfs_mem_close(struct rmtfs_mem *rmem);
+int64_t rmtfs_mem_alloc(struct rmtfs_mem *rmem, size_t size);
+void rmtfs_mem_free(struct rmtfs_mem *rmem);
+ssize_t rmtfs_mem_read(struct rmtfs_mem *rmem, unsigned long phys_address, void *buf, ssize_t len);
+ssize_t rmtfs_mem_write(struct rmtfs_mem *rmem, unsigned long phys_address, const void *buf, ssize_t len);
 
 int storage_open(void);
 int storage_get(unsigned node, const char *path);

sharedmem.c

@@ -1,90 +1,249 @@
 #include <sys/mman.h>
+#include <sys/stat.h>
 #include <sys/types.h>
 #include <dirent.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <libudev.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <limits.h>
 #include <string.h>
 #include <unistd.h>
 #include "rmtfs.h"
 
-static int rmtfs_mem_enumerate(void);
+static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem);
 
-static uint64_t rmtfs_mem_address;
-static uint64_t rmtfs_mem_size;
-static void *rmtfs_mem_base;
-static int rmtfs_mem_fd;
-
-int rmtfs_mem_open(void)
-{
+struct rmtfs_mem {
+	uint64_t address;
+	uint64_t size;
 	void *base;
-	int ret;
 	int fd;
+};
 
-	ret = rmtfs_mem_enumerate();
-	if (ret < 0)
-		return ret;
+static int parse_hex_sysattr(struct udev_device *dev, const char *name,
+			     unsigned long *value)
+{
+	unsigned long val;
+	const char *buf;
+	char *endptr;
 
-	fd = open("/dev/mem", O_RDWR|O_SYNC);
-	if (fd < 0) {
-		fprintf(stderr, "failed to open /dev/mem\n");
-		return fd;
-	}
+	buf = udev_device_get_sysattr_value(dev, name);
+	if (!buf)
+		return -ENOENT;
 
-	base = mmap(0, rmtfs_mem_size, PROT_READ|PROT_WRITE, MAP_SHARED, fd, rmtfs_mem_address);
-	if (base == MAP_FAILED) {
-		fprintf(stderr, "failed to mmap: %s\n", strerror(errno));
+	errno = 0;
+	val = strtoul(buf, &endptr, 16);
+	if ((val == LONG_MAX && errno == ERANGE) || endptr == buf) {
 		return -errno;
 	}
 
-	rmtfs_mem_base = base;
-	rmtfs_mem_fd = fd;
+	*value = val;
 
 	return 0;
 }
 
-int64_t rmtfs_mem_alloc(size_t alloc_size)
+static int rmtfs_mem_open_rfsa(struct rmtfs_mem *rmem, int client_id)
 {
-	if (alloc_size > rmtfs_mem_size) {
+	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;
+	void *base;
+	int ret;
+	int fd;
+
+	rmem = malloc(sizeof(*rmem));
+	if (!rmem)
+		return NULL;
+
+	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");
+
+		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;
+	}
+
+	return rmem;
+
+err_close_fd:
+	close(fd);
+err:
+	free(rmem);
+	return NULL;
+}
+
+int64_t rmtfs_mem_alloc(struct rmtfs_mem *rmem, size_t alloc_size)
+{
+	if (alloc_size > rmem->size) {
 		fprintf(stderr,
 			"[RMTFS] rmtfs shared memory not large enough for allocation request 0x%zx vs 0x%lx\n",
-			alloc_size, rmtfs_mem_size);
+			alloc_size, rmem->size);
 		return -EINVAL;
 	}
 
-	return rmtfs_mem_address;
+	return rmem->address;
 }
 
-void rmtfs_mem_free(void)
+void rmtfs_mem_free(struct rmtfs_mem *rmem)
 {
 }
 
-void *rmtfs_mem_ptr(unsigned phys_address, size_t len)
+static void *rmtfs_mem_ptr(struct rmtfs_mem *rmem, unsigned phys_address, ssize_t len)
 {
 	uint64_t start;
 	uint64_t end;
 
+	if (len < 0)
+		return NULL;
+
 	start = phys_address;
 	end = start + len;
 
-	if (start < rmtfs_mem_address || end > rmtfs_mem_address + rmtfs_mem_size)
+	if (start < rmem->address || end > rmem->address + rmem->size)
 		return NULL;
 
-	return rmtfs_mem_base + phys_address - rmtfs_mem_address;
+	return rmem->base + phys_address - rmem->address;
 }
 
-void rmtfs_mem_close(void)
+ssize_t rmtfs_mem_read(struct rmtfs_mem *rmem, unsigned long phys_address, void *buf, ssize_t len)
 {
-	munmap(rmtfs_mem_base, rmtfs_mem_size);
-	close(rmtfs_mem_fd);
+	off_t offset;
+	void *ptr;
 
-	rmtfs_mem_fd = -1;
-	rmtfs_mem_base = MAP_FAILED;
+	if (rmem->base) {
+		ptr = rmtfs_mem_ptr(rmem, phys_address, len);
+		if (!ptr)
+			return -EINVAL;
+
+		memcpy(buf, ptr, len);
+	} else {
+		offset = phys_address - rmem->address;
+		len = pread(rmem->fd, buf, len, offset);
+	}
+
+	return len;
 }
 
-static int rmtfs_mem_enumerate(void)
+ssize_t rmtfs_mem_write(struct rmtfs_mem *rmem, unsigned long phys_address, const void *buf, ssize_t len)
+{
+	off_t offset;
+	void *ptr;
+
+	if (rmem->base) {
+		ptr = rmtfs_mem_ptr(rmem, phys_address, len);
+		if (!ptr)
+			return -EINVAL;
+
+		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)
+{
+	if (rmem->base)
+		munmap(rmem->base, rmem->size);
+
+	close(rmem->fd);
+
+	free(rmem);
+}
+
+static int rmtfs_mem_enumerate(struct rmtfs_mem *rmem)
 {
 	union {
 		uint32_t dw[2];
@@ -130,11 +289,11 @@ static int rmtfs_mem_enumerate(void)
 
 		n = read(regfd, &reg, sizeof(reg));
 		if (n == 2 * sizeof(uint32_t)) {
-			rmtfs_mem_address = be32toh(reg.dw[0]);
-			rmtfs_mem_size = be32toh(reg.dw[1]);
+			rmem->address = be32toh(reg.dw[0]);
+			rmem->size = be32toh(reg.dw[1]);
 		} else if (n == 2 * sizeof(uint64_t)) {
-			rmtfs_mem_address = be64toh(reg.qw[0]);
-			rmtfs_mem_size = be64toh(reg.qw[1]);
+			rmem->address = be64toh(reg.qw[0]);
+			rmem->size = be64toh(reg.qw[1]);
 		} else {
 			fprintf(stderr, "failed to read reg of %s: %s\n",
 				de->d_name, strerror(-errno));

storage.c

@@ -19,6 +19,7 @@ struct caller {
 	unsigned node;
 	int fd;
 	unsigned dev_error;
+	const struct partition *partition;
 };
 
 static const struct partition partition_table[] = {
@@ -46,7 +47,7 @@ int storage_open(void)
 int storage_get(unsigned node, const char *path)
 {
 	const struct partition *part;
-	struct caller *caller;
+	struct caller *caller = NULL;
 	int saved_errno;
 	int fd;
 	int i;
@@ -60,6 +61,14 @@ int storage_get(unsigned node, const char *path)
 	return -EPERM;
 
 found:
+	/* Check if this node already has the requested path open */
+	for (i = 0; i < MAX_CALLERS; i++) {
+		if (caller_handles[i].fd != -1 &&
+		    caller_handles[i].node == node &&
+		    caller_handles[i].partition == part)
+			return caller_handles[i].id;
+	}
+
 	for (i = 0; i < MAX_CALLERS; i++) {
 		if (caller_handles[i].fd == -1) {
 			caller = &caller_handles[i];
@@ -81,6 +90,7 @@ found:
 
 	caller->node = node;
 	caller->fd = fd;
+	caller->partition = part;
 
 	return caller->id;
 }
@@ -98,6 +108,8 @@ int storage_put(unsigned node, int caller_id)
 
 	close(caller->fd);
 	caller->fd = -1;
+	caller->partition = NULL;
+
 	return 0;
 }