diag: Add dynamic peripheral support

Add support for detecting peripherals as they come and go.

Signed-off-by: Bjorn Andersson <bjorn.andersson@linaro.org>

Makefile

@@ -1,13 +1,13 @@
 DIAG := diag
 
 CFLAGS := -Wall -g -O2
-LDFLAGS :=
+LDFLAGS := -ludev
 
-SRCS := crc_ccitt.c diag.c diag_cntl.c  mbuf.c util.c watch.c
+SRCS := crc_ccitt.c diag.c diag_cntl.c mbuf.c peripheral.c util.c watch.c
 OBJS := $(SRCS:.c=.o)
 
 $(DIAG): $(OBJS)
-	$(CC) $(LDFLAGS) -o $@ $^
+	$(CC) -o $@ $^ $(LDFLAGS)
 
 install: $(DIAG)
 	install -D -m 755 $< $(DESTDIR)$(prefix)/bin/$<

diag.c

@@ -19,6 +19,7 @@
 #include <err.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <libudev.h>
 #include <netdb.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -31,6 +32,7 @@
 #include "diag_cntl.h"
 #include "list.h"
 #include "mbuf.h"
+#include "peripheral.h"
 #include "util.h"
 #include "watch.h"
 
@@ -147,7 +149,7 @@ void queue_push(struct list_head *queue, uint8_t *msg, size_t msglen)
 	list_add(queue, &mbuf->node);
 }
 
-static int diag_data_recv(int fd, void *data)
+int diag_data_recv(int fd, void *data)
 {
 	struct peripheral *peripheral = data;
 	struct diag_client *client;
@@ -159,32 +161,38 @@ static int diag_data_recv(int fd, void *data)
 	size_t len;
 	ssize_t n;
 
-	n = read(fd, buf, sizeof(buf));
-	if (n < 0) {
-		warn("failed to read from data channel");
-		return n;
-	}
-
-	ptr = buf;
-	len = n;
 	for (;;) {
-		if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE) {
-			msg = ptr;
-			msglen = len;
-		} else {
-			msg = hdlc_decode_one(&ptr, &len, &msglen);
-			if (!msg)
+		n = read(fd, buf, sizeof(buf));
+		if (n < 0) {
+			if (errno != EAGAIN) {
+				warn("failed to read from data channel");
+				peripheral_close(peripheral);
+			}
+
+			break;
+		}
+
+		ptr = buf;
+		len = n;
+		for (;;) {
+			if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE) {
+				msg = ptr;
+				msglen = len;
+			} else {
+				msg = hdlc_decode_one(&ptr, &len, &msglen);
+				if (!msg)
+					break;
+			}
+
+			list_for_each(item, &diag_clients) {
+				client = container_of(item, struct diag_client, node);
+
+				queue_push(&client->outq, msg, msglen);
+			}
+
+			if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE)
 				break;
 		}
-
-		list_for_each(item, &diag_clients) {
-			client = container_of(item, struct diag_client, node);
-
-			hdlc_enqueue(&client->outq, msg, msglen);
-		}
-
-		if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE)
-			break;
 	}
 
 	return 0;
@@ -311,8 +319,6 @@ static int diag_sock_recv(int fd, void *data)
 
 int main(int argc, char **argv)
 {
-	struct peripheral *hexagon;
-	struct peripheral *wcnss;
 	struct diag_client *qxdm;
 	int ret;
 
@@ -329,46 +335,7 @@ int main(int argc, char **argv)
 	watch_add_writeq(qxdm->fd, &qxdm->outq);
 	list_add(&diag_clients, &qxdm->node);
 
-	hexagon = malloc(sizeof(*hexagon));
-	memset(hexagon, 0, sizeof(*hexagon));
-	hexagon->name = "hexagon";
-
-	wcnss = malloc(sizeof(*wcnss));
-	memset(wcnss, 0, sizeof(*wcnss));
-	wcnss->name = "wcnss";
-
-	hexagon->cntl_fd = open("/dev/rpmsg/hexagon/DIAG_CNTL", O_RDWR | O_NONBLOCK);
-	if (hexagon->cntl_fd < 0)
-		err(1, "failed to open DIAG_CNTL");
-
-	hexagon->data_fd = open("/dev/rpmsg/hexagon/DIAG", O_RDWR | O_NONBLOCK);
-	if (hexagon->data_fd < 0)
-		err(1, "failed to open DIAG");
-
-	hexagon->cmd_fd = open("/dev/rpmsg/hexagon/DIAG_CMD", O_RDWR | O_NONBLOCK);
-	if (hexagon->cmd_fd < 0)
-		err(1, "failed to open DIAG_CMD");
-
-	wcnss->cntl_fd = open("/dev/rpmsg/pronto/APPS_RIVA_CTRL", O_RDWR | O_NONBLOCK);
-	if (wcnss->cntl_fd < 0)
-		err(1, "failed to open APPS_RIVA_CTRL");
-
-	wcnss->data_fd = open("/dev/rpmsg/pronto/APPS_RIVA_DATA", O_RDWR | O_NONBLOCK);
-	if (wcnss->data_fd < 0)
-		err(1, "failed to open APPS_RIVA_DATA");
-
-	watch_add_readfd(hexagon->cntl_fd, diag_cntl_recv, hexagon);
-	watch_add_writeq(hexagon->cntl_fd, &hexagon->cntlq);
-	watch_add_readfd(hexagon->data_fd, diag_data_recv, hexagon);
-	watch_add_writeq(hexagon->data_fd, &hexagon->dataq);
-
-	watch_add_readfd(wcnss->cntl_fd, diag_cntl_recv, wcnss);
-	watch_add_writeq(wcnss->cntl_fd, &wcnss->cntlq);
-	watch_add_readfd(wcnss->data_fd, diag_data_recv, wcnss);
-	watch_add_writeq(wcnss->data_fd, &wcnss->dataq);
-
-	diag_cntl_send_feature_mask(hexagon);
-	diag_cntl_send_feature_mask(wcnss);
+	peripheral_init();
 
 	watch_run();
 

diag.h

@@ -36,7 +36,9 @@ struct diag_client {
 };
 
 struct peripheral {
-	const char *name;
+	struct list_head  node;
+
+	char *name;
 
 	unsigned long features;
 
@@ -61,4 +63,6 @@ void queue_push(struct list_head *queue, uint8_t *msg, size_t msglen);
 
 extern struct list_head diag_cmds;
 
+int diag_data_recv(int fd, void *data);
+
 #endif

diag_cntl.c

@@ -19,6 +19,8 @@
 #include <string.h>
 #include <unistd.h>
 #include "diag.h"
+#include "diag_cntl.h"
+#include "peripheral.h"
 #include "util.h"
 
 #define __packed __attribute__((packed))
@@ -157,6 +159,8 @@ static int diag_cntl_feature_mask(struct peripheral *peripheral,
 
 	peripheral->features = mask;
 
+	diag_cntl_send_feature_mask(peripheral);
+
 	return 0;
 }
 
@@ -190,7 +194,8 @@ int diag_cntl_recv(int fd, void *data)
 	n = read(fd, buf, sizeof(buf));
 	if (n < 0) {
 		warn("failed to read from cntl channel");
-		return n;
+		peripheral_close(peripheral);
+		return 0;
 	}
 
 	for (;;) {
@@ -224,3 +229,17 @@ int diag_cntl_recv(int fd, void *data)
 
 	return 0;
 }
+
+void diag_cntl_close(struct peripheral *peripheral)
+{
+	struct list_head *item;
+	struct list_head *next;
+	struct diag_cmd *dc;
+
+	list_for_each_safe(item, next, &diag_cmds) {
+		dc = container_of(item, struct diag_cmd, node);
+		if (dc->peripheral == peripheral)
+			list_del(&dc->node);
+	}
+
+}

diag_cntl.h

@@ -5,5 +5,6 @@
 
 int diag_cntl_recv(int fd, void *data);
 void diag_cntl_send_feature_mask(struct peripheral *peripheral);
+void diag_cntl_close(struct peripheral *peripheral);
 
 #endif

list.h

@@ -40,4 +40,7 @@ static inline void list_del(struct list_head *item)
 #define list_for_each(item, list) \
 	for (item = (list)->next; item != list; item = item->next)
 
+#define list_for_each_safe(item, next, list) \
+	for (item = (list)->next, next = item->next; item != list; item = next, next = item->next)
+
 #endif

peripheral.c

@@ -0,0 +1,305 @@
+/*
+ * Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2016, Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+#include <err.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <libudev.h>
+#include <netdb.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+
+#include "diag.h"
+#include "diag_cntl.h"
+#include "list.h"
+#include "peripheral.h"
+#include "util.h"
+#include "watch.h"
+
+struct devnode {
+	char *devnode;
+	char *name;
+	char *rproc;
+
+	struct list_head node;
+};
+
+struct list_head peripherals = LIST_INIT(peripherals);
+struct list_head devnodes = LIST_INIT(devnodes);
+
+static struct devnode *devnode_get(const char *devnode)
+{
+	struct list_head *item;
+	struct devnode *node;
+
+	list_for_each(item, &devnodes) {
+		node = container_of(item, struct devnode, node);
+		if (strcmp(node->devnode, devnode) == 0)
+			return node;
+	}
+
+	return NULL;
+}
+
+static int devnode_open(const char *rproc, const char *name)
+{
+	struct list_head *item;
+	struct devnode *node;
+
+	list_for_each(item, &devnodes) {
+		node = container_of(item, struct devnode, node);
+		if (strcmp(node->rproc, rproc) == 0 &&
+		    strcmp(node->name, name) == 0)
+			return open(node->devnode, O_RDWR);
+	}
+
+	return -1;
+}
+
+static void devnode_add(const char *devnode, const char *name, const char *rproc)
+{
+	struct devnode *node;
+
+	node = devnode_get(devnode);
+	if (node) {
+		warnx("node already in list");
+		return;
+	}
+
+	node = malloc(sizeof(*node));
+	memset(node, 0, sizeof(*node));
+
+	node->devnode = strdup(devnode);
+	node->name = strdup(name);
+	node->rproc = strdup(rproc);
+
+	list_add(&devnodes, &node->node);
+}
+
+static void devnode_remove(const char *devnode)
+{
+	struct devnode *node;
+
+	node = devnode_get(devnode);
+	if (!node)
+		return;
+
+	list_del(&node->node);
+
+	free(node->name);
+	free(node->devnode);
+	free(node->rproc);
+}
+
+static const char *peripheral_udev_get_name(struct udev_device *dev)
+{
+	return udev_device_get_sysattr_value(dev, "name");
+}
+
+static const char *peripheral_udev_get_remoteproc(struct udev_device *dev)
+{
+	struct udev_device *parent;
+	const char *p;
+
+	parent = udev_device_get_parent(dev);
+	if (!parent)
+		return NULL;
+
+	p = udev_device_get_sysattr_value(parent, "rpmsg_name");
+	if (p)
+		return p;
+
+	return peripheral_udev_get_remoteproc(parent);
+}
+
+static void peripheral_open(void *data)
+{
+	struct peripheral *peripheral = data;
+	char *rproc = peripheral->name;
+	int ret;
+	int fd;
+
+	fd = devnode_open(rproc, "DIAG");
+	if (fd < 0)
+		fd = devnode_open(rproc, "APPS_RIVA_DATA");
+	if (fd < 0) {
+		warn("unable to open DIAG channel\n");
+		return;
+	}
+	peripheral->data_fd = fd;
+
+	fd = devnode_open(rproc, "DIAG_CNTL");
+	if (fd < 0)
+		fd = devnode_open(rproc, "APPS_RIVA_CTRL");
+	if (fd < 0) {
+		warn("unable to find DIAG_CNTL channel\n");
+		close(peripheral->data_fd);
+		peripheral->data_fd = -1;
+		return;
+	}
+	peripheral->cntl_fd = fd;
+
+	fd = devnode_open(rproc, "DIAG_CMD");
+	if (fd >= 0)
+		peripheral->cmd_fd = fd;
+
+	ret = fcntl(peripheral->data_fd, F_SETFL, O_NONBLOCK);
+	if (ret < 0)
+		warn("failed to turn DIAG non blocking");
+
+	watch_add_writeq(peripheral->cntl_fd, &peripheral->cntlq);
+	watch_add_writeq(peripheral->data_fd, &peripheral->dataq);
+	watch_add_readfd(peripheral->cntl_fd, diag_cntl_recv, peripheral);
+	watch_add_readfd(peripheral->data_fd, diag_data_recv, peripheral);
+}
+
+static int peripheral_create(const char *name)
+{
+	struct peripheral *peripheral;
+	struct list_head *item;
+
+	list_for_each(item, &peripherals) {
+		peripheral = container_of(item, struct peripheral, node);
+		if (strcmp(peripheral->name, name) == 0)
+			return 0;
+	}
+
+	peripheral = malloc(sizeof(*peripheral));
+	memset(peripheral, 0, sizeof(*peripheral));
+
+	peripheral->name = strdup(name);
+	peripheral->data_fd = -1;
+	peripheral->cntl_fd = -1;
+	peripheral->cmd_fd = -1;
+	list_add(&peripherals, &peripheral->node);
+
+	watch_add_timer(peripheral_open, peripheral, 1000, false);
+
+	return 0;
+}
+
+void peripheral_close(struct peripheral *peripheral)
+{
+	diag_cntl_close(peripheral);
+
+	watch_remove_fd(peripheral->data_fd);
+	watch_remove_fd(peripheral->cntl_fd);
+	watch_remove_fd(peripheral->cmd_fd);
+
+	close(peripheral->data_fd);
+	close(peripheral->cntl_fd);
+	close(peripheral->cmd_fd);
+
+	list_del(&peripheral->node);
+	free(peripheral->name);
+	free(peripheral);
+}
+
+static int peripheral_udev_update(int fd, void *data)
+{
+	struct udev_monitor *mon = data;
+	struct udev_device *dev;
+	const char *devnode;
+	const char *action;
+	const char *rproc;
+	const char *name;
+
+	dev = udev_monitor_receive_device(mon);
+	if (!dev)
+		return 0;
+
+	action = udev_device_get_action(dev);
+	devnode = udev_device_get_devnode(dev);
+
+	if (!devnode)
+		goto unref_dev;
+
+	if (strcmp(action, "add") == 0) {
+		name = peripheral_udev_get_name(dev);
+		rproc = peripheral_udev_get_remoteproc(dev);
+
+		if (!name || !rproc)
+			goto unref_dev;
+
+		devnode_add(devnode, name, rproc);
+
+		peripheral_create(rproc);
+	} else if (strcmp(action, "remove") == 0) {
+		devnode_remove(devnode);
+	} else {
+		warn("unknown udev action");
+	}
+
+unref_dev:
+	udev_device_unref(dev);
+
+	return 0;
+}
+
+int peripheral_init(void)
+{
+	struct udev_list_entry *devices;
+	struct udev_list_entry *entry;
+	struct udev_enumerate *enu;
+	struct udev_monitor *mon;
+	struct udev_device *dev;
+	struct udev *udev;
+	const char *devnode;
+	const char *path;
+	const char *rproc;
+	const char *name;
+	int fd;
+
+	udev = udev_new();
+	if (!udev)
+		err(1, "failed to initialize libudev");
+
+	mon = udev_monitor_new_from_netlink(udev, "udev");
+	udev_monitor_filter_add_match_subsystem_devtype(mon, "rpmsg", NULL);
+	udev_monitor_enable_receiving(mon);
+
+	fd = udev_monitor_get_fd(mon);
+
+	enu = udev_enumerate_new(udev);
+	udev_enumerate_add_match_subsystem(enu, "rpmsg");
+	udev_enumerate_scan_devices(enu);
+
+	devices = udev_enumerate_get_list_entry(enu);
+	udev_list_entry_foreach(entry, devices) {
+		path = udev_list_entry_get_name(entry);
+		dev = udev_device_new_from_syspath(udev, path);
+
+		devnode = udev_device_get_devnode(dev);
+		name = peripheral_udev_get_name(dev);
+		rproc = peripheral_udev_get_remoteproc(dev);
+
+		if (devnode && name && rproc) {
+			devnode_add(devnode, name, rproc);
+			peripheral_create(rproc);
+		}
+
+		udev_device_unref(dev);
+	}
+
+	watch_add_readfd(fd, peripheral_udev_update, mon);
+
+	return 0;
+}

peripheral.h

@@ -0,0 +1,7 @@
+#ifndef __PERIPHERAL_H__
+#define __PERIPHERAL_H__
+
+int peripheral_init(void);
+void peripheral_close(struct peripheral *peripheral);
+
+#endif

watch.c

@@ -104,6 +104,29 @@ int watch_add_writeq(int fd, struct list_head *queue)
 	return 0;
 }
 
+void watch_remove_fd(int fd)
+{
+	struct list_head *item;
+	struct list_head *next;
+	struct watch *w;
+
+	list_for_each_safe(item, next, &read_watches) {
+		w = container_of(item, struct watch, node);
+		if (w->fd == fd) {
+			list_del(&w->node);
+			free(w);
+		}
+	}
+
+	list_for_each_safe(item, next, &write_watches) {
+		w = container_of(item, struct watch, node);
+		if (w->fd == fd) {
+			list_del(&w->node);
+			free(w);
+		}
+	}
+}
+
 int watch_add_quit(int (*cb)(int, void*), void *data)
 {
 	struct watch *w;
@@ -235,16 +258,15 @@ void watch_run(void)
 			w = container_of(item, struct watch, node);
 			FD_SET(w->fd, &rfds);
 
-			if (w->fd >= nfds)
-				nfds = w->fd + 1;
+			nfds = MAX(w->fd + 1, nfds);
 		}
 
 		list_for_each(item, &write_watches) {
 			w = container_of(item, struct watch, node);
 			if (!list_empty(w->queue)) {
 				FD_SET(w->fd, &wfds);
-				if (w->fd >= nfds)
-					nfds = w->fd + 1;
+
+				nfds = MAX(w->fd + 1, nfds);
 			}
 		}
 

watch.h

@@ -6,6 +6,7 @@
 
 int watch_add_readfd(int fd, int (*cb)(int, void*), void *data);
 int watch_add_writeq(int fd, struct list_head *queue);
+void watch_remove_fd(int fd);
 int watch_add_quit(int (*cb)(int, void*), void *data);
 int watch_add_timer(void (*cb)(void *), void *data,
 		    unsigned int interval, bool repeat);