1 files changed, 567 insertions, 0 deletions
diff --git a/extcap/dpauxmon.c b/extcap/dpauxmon.c
new file mode 100644
index 0000000000..487401d38e
--- /dev/null
+++ b/extcap/dpauxmon.c
@@ -0,0 +1,567 @@
+/* dpauxmon.c
+ * dpauxmon is an extcap tool used to monitor DisplayPort AUX channel traffic
+ * coming in from the kernel via generic netlink
+ * Copyright 2018, Dirk Eibach, Guntermann & Drunck GmbH <dirk.eibach@gdsys.cc>
+ *
+ * Wireshark - Network traffic analyzer
+ * By Gerald Combs <gerald@wireshark.org>
+ * Copyright 1998 Gerald Combs
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+#include "config.h"
+
+#include "extcap-base.h"
+
+#include <wsutil/strtoi.h>
+#include <wsutil/filesystem.h>
+#include <writecap/pcapio.h>
+
+#include <netlink/netlink.h>
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+#include <netlink/genl/mngt.h>
+
+#include <signal.h>
+#include <errno.h>
+
+#include <linux/genetlink.h>
+
+#include "dpauxmon_user.h"
+
+#define PCAP_SNAPLEN 128
+
+#define DPAUXMON_EXTCAP_INTERFACE "dpauxmon"
+#define DPAUXMON_VERSION_MAJOR "0"
+#define DPAUXMON_VERSION_MINOR "1"
+#define DPAUXMON_VERSION_RELEASE "0"
+
+static gboolean run_loop = TRUE;
+FILE* pcap_fp = NULL;
+
+enum {
+	EXTCAP_BASE_OPTIONS_ENUM,
+	OPT_HELP,
+	OPT_VERSION,
+	OPT_INTERFACE_ID,
+};
+
+static struct option longopts[] = {
+	EXTCAP_BASE_OPTIONS,
+	/* Generic application options */
+	{ "help", no_argument, NULL, OPT_HELP},
+	{ "version", no_argument, NULL, OPT_VERSION},
+	/* Interfaces options */
+	{ "interface_id", required_argument, NULL, OPT_INTERFACE_ID},
+	{ 0, 0, 0, 0 }
+};
+
+static struct nla_policy dpauxmon_attr_policy[DPAUXMON_ATTR_MAX + 1] = {
+	[DPAUXMON_ATTR_IFINDEX] = { .type = NLA_U32 },
+	[DPAUXMON_ATTR_FROM_SOURCE] = { .type = NLA_FLAG },
+	[DPAUXMON_ATTR_TIMESTAMP] = { .type = NLA_MSECS },
+};
+
+struct family_handler_args {
+	const char *group;
+	int id;
+};
+
+static int list_config(char *interface)
+{
+	unsigned inc = 0;
+
+	if (!interface) {
+		g_warning("No interface specified.");
+		return EXIT_FAILURE;
+	}
+
+	if (g_strcmp0(interface, DPAUXMON_EXTCAP_INTERFACE)) {
+		g_warning("interface must be %s", DPAUXMON_EXTCAP_INTERFACE);
+		return EXIT_FAILURE;
+	}
+
+	printf("arg {number=%u}{call=--interface_id}{display=Interface index}"
+		"{type=unsigned}{range=1,65535}{default=%u}{tooltip=The dpauxmon interface index}\n",
+		inc++, 0);
+
+	extcap_config_debug(&inc);
+
+	return EXIT_SUCCESS;
+}
+
+static void exit_from_loop(int signo _U_)
+{
+	run_loop = FALSE;
+}
+
+static int setup_dumpfile(const char* fifo, FILE** fp)
+{
+	guint64 bytes_written = 0;
+	int err;
+
+	if (!g_strcmp0(fifo, "-")) {
+		*fp = stdout;
+		return EXIT_SUCCESS;
+	}
+
+	*fp = fopen(fifo, "w");
+	if (!(*fp)) {
+		g_warning("Error creating output file: %s", g_strerror(errno));
+		return EXIT_FAILURE;
+	}
+
+	if (!libpcap_write_file_header(*fp, 275, PCAP_SNAPLEN, FALSE, &bytes_written, &err)) {
+		g_warning("Can't write pcap file header");
+		return EXIT_FAILURE;
+	}
+
+	return EXIT_SUCCESS;
+}
+
+static int dump_packet(FILE* fp, const char* buf, const ssize_t buflen, guint64 ts_usecs)
+{
+	guint64 bytes_written = 0;
+	int err;
+	int ret = EXIT_SUCCESS;
+
+	if (!libpcap_write_packet(fp, ts_usecs / 1000000, ts_usecs % 1000000, buflen, buflen, buf, &bytes_written, &err)) {
+		g_warning("Can't write packet");
+		ret = EXIT_FAILURE;
+	}
+
+	fflush(fp);
+
+	return ret;
+}
+
+static int error_handler(struct sockaddr_nl *nla _U_, struct nlmsgerr *err,
+			 void *arg)
+{
+	int *ret = (int*)arg;
+	*ret = err->error;
+	return NL_STOP;
+}
+
+static int ack_handler(struct nl_msg *msg _U_, void *arg)
+{
+	int *ret = (int*)arg;
+	*ret = 0;
+	return NL_STOP;
+}
+
+static int family_handler(struct nl_msg *msg, void *arg)
+{
+	struct family_handler_args *grp = (struct family_handler_args *)arg;
+	struct nlattr *tb[CTRL_ATTR_MAX + 1];
+	struct genlmsghdr *gnlh = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
+	struct nlattr *mcgrp;
+	int rem_mcgrp;
+
+	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+		  genlmsg_attrlen(gnlh, 0), NULL);
+
+	if (!tb[CTRL_ATTR_MCAST_GROUPS])
+		return NL_SKIP;
+
+	nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
+		struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
+
+		nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
+			  (struct nlattr *)nla_data(mcgrp), nla_len(mcgrp), NULL);
+
+		if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
+		    !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
+			continue;
+
+		if (strncmp((const char*)nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
+			    grp->group,
+			    nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
+			continue;
+
+		grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
+
+		break;
+	}
+
+	return NL_SKIP;
+}
+
+static int nl_get_multicast_id(struct nl_sock *sock, int family,
+			       const char *group)
+{
+	struct nl_msg *msg;
+	struct nl_cb *cb;
+	int ret, ctrlid;
+	struct family_handler_args grp = {
+		.group = group,
+		.id = -ENOENT,
+	};
+
+	msg = nlmsg_alloc();
+	if (!msg)
+		return -ENOMEM;
+
+	cb = nl_cb_alloc(NL_CB_DEFAULT);
+	if (!cb) {
+		ret = -ENOMEM;
+		goto out_fail_cb;
+	}
+
+	ctrlid = genl_ctrl_resolve(sock, "nlctrl");
+
+	genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
+
+	ret = -ENOBUFS;
+	NLA_PUT_U16(msg, CTRL_ATTR_FAMILY_ID, family);
+
+	ret = nl_send_auto_complete(sock, msg);
+	if (ret < 0)
+		goto nla_put_failure;
+
+	ret = 1;
+
+	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &ret);
+	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &ret);
+	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, family_handler, &grp);
+
+	while (ret > 0)
+		nl_recvmsgs(sock, cb);
+
+	if (ret == 0)
+		ret = grp.id;
+nla_put_failure:
+	nl_cb_put(cb);
+out_fail_cb:
+	nlmsg_free(msg);
+	return ret;
+}
+
+/*
+ * netlink callback handlers
+ */
+
+int nl_receive_timeout(struct nl_sock* sk, struct sockaddr_nl* nla, unsigned char** buf, struct ucred** creds)
+{
+	struct pollfd fds = {nl_socket_get_fd(sk), POLLIN, 0};
+	int poll_res = poll(&fds, 1, 500);
+
+	if (poll_res < 0) {
+		g_debug("poll() failed in nl_receive_timeout");
+		g_usleep(500000);
+		return -nl_syserr2nlerr(errno);
+	}
+
+	return poll_res ? nl_recv(sk, nla, buf, creds) : 0;
+}
+
+static int send_start(struct nl_sock *sock, int family, unsigned int interface_id)
+{
+	struct nl_msg *msg;
+	void *hdr;
+	int err;
+	int res = 0;
+
+	msg = nlmsg_alloc();
+	if (msg == NULL) {
+		g_critical("Unable to allocate netlink message");
+		return -ENOMEM;
+	}
+
+	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, 0,
+		    DPAUXMON_CMD_START, 1);
+	if (hdr == NULL) {
+		g_critical("Unable to write genl header");
+		res = -ENOMEM;
+		goto out_free;
+	}
+
+	if ((err = nla_put_u32(msg, DPAUXMON_ATTR_IFINDEX, interface_id)) < 0) {
+		g_critical("Unable to add attribute: %s", nl_geterror(err));
+		res = -EIO;
+		goto out_free;
+	}
+
+	if ((err = nl_send_auto_complete(sock, msg)) < 0)
+		g_debug("Starting monitor failed, already running?");
+
+out_free:
+	nlmsg_free(msg);
+	return res;
+}
+
+static void send_stop(struct nl_sock *sock, int family, unsigned int interface_id)
+{
+	struct nl_msg *msg;
+	void *hdr;
+	int err;
+
+	msg = nlmsg_alloc();
+	if (msg == NULL) {
+		g_critical("Unable to allocate netlink message");
+		return;
+	}
+
+	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, family, 0, 0,
+		    DPAUXMON_CMD_STOP, 1);
+	if (hdr == NULL) {
+		g_critical("Unable to write genl header");
+		goto out_free;
+	}
+
+	if ((err = nla_put_u32(msg, DPAUXMON_ATTR_IFINDEX, interface_id)) < 0) {
+		g_critical("Unable to add attribute: %s", nl_geterror(err));
+		goto out_free;
+	}
+
+	if ((err = nl_send_auto_complete(sock, msg)) < 0) {
+		g_critical("Unable to send message: %s", nl_geterror(err));
+		goto out_free;
+	}
+
+out_free:
+	nlmsg_free(msg);
+}
+
+static int handle_data(struct nl_cache_ops *unused _U_, struct genl_cmd *cmd _U_,
+			 struct genl_info *info, void *arg _U_)
+{
+	unsigned char *data;
+	int data_size;
+	guint64 ts = 0;
+	guint8 packet[21] = { 0x00 };
+
+	if (!info->attrs[DPAUXMON_ATTR_DATA])
+		return NL_SKIP;
+
+	data = (unsigned char*)nla_data(info->attrs[DPAUXMON_ATTR_DATA]);
+	data_size = nla_len(info->attrs[DPAUXMON_ATTR_DATA]);
+
+	if (data_size > 19) {
+		g_debug("Invalid packet size %u", data_size);
+		return NL_SKIP;
+	}
+
+	if (info->attrs[DPAUXMON_ATTR_TIMESTAMP])
+		ts = nla_get_msecs(info->attrs[DPAUXMON_ATTR_TIMESTAMP]);
+
+	packet[1] = info->attrs[DPAUXMON_ATTR_FROM_SOURCE] ? 0x01 : 0x00;
+
+	memcpy(&packet[2], data, data_size);
+
+	if (dump_packet(pcap_fp, packet, data_size + 2, ts) == EXIT_FAILURE)
+		run_loop = FALSE;
+
+	return NL_OK;
+}
+
+static int parse_cb(struct nl_msg *msg, void *arg _U_)
+{
+	return genl_handle_msg(msg, NULL);
+}
+
+static struct genl_cmd cmds[] = {
+#if 0
+	{
+		.c_id		= DPAUXMON_CMD_START,
+		.c_name		= "dpauxmon start",
+		.c_maxattr	= DPAUXMON_ATTR_MAX,
+		.c_attr_policy	= dpauxmon_attr_policy,
+		.c_msg_parser	= &handle_start,
+	},
+	{
+		.c_id		= DPAUXMON_CMD_STOP,
+		.c_name		= "dpauxmon stop",
+		.c_maxattr	= DPAUXMON_ATTR_MAX,
+		.c_attr_policy	= dpauxmon_attr_policy,
+		.c_msg_parser	= &handle_stop,
+	},
+#endif
+	{
+		.c_id		= DPAUXMON_CMD_DATA,
+		.c_name		= "dpauxmon data",
+		.c_maxattr	= DPAUXMON_ATTR_MAX,
+		.c_attr_policy	= dpauxmon_attr_policy,
+		.c_msg_parser	= &handle_data,
+	},
+};
+
+#define ARRAY_SIZE(X) (sizeof(X) / sizeof((X)[0]))
+
+static struct genl_ops ops = {
+	.o_name = "dpauxmon",
+	.o_cmds = cmds,
+	.o_ncmds = ARRAY_SIZE(cmds),
+};
+
+struct nl_sock *sock;
+
+static void run_listener(const char* fifo, unsigned int interface_id)
+{
+	int err;
+	int grp;
+	struct sigaction int_handler = { .sa_handler = exit_from_loop };
+	struct nl_cb *socket_cb;
+
+	if (sigaction(SIGINT, &int_handler, 0)) {
+		g_warning("Can't set signal handler");
+		return;
+	}
+
+	if (setup_dumpfile(fifo, &pcap_fp) == EXIT_FAILURE) {
+		if (pcap_fp)
+			goto close_out;
+	}
+
+	if (!(sock = nl_socket_alloc())) {
+		g_critical("Unable to allocate netlink socket");
+		goto close_out;
+	}
+
+	if ((err = nl_connect(sock, NETLINK_GENERIC)) < 0) {
+		g_critical("Unable to connect netlink socket: %s",
+			   nl_geterror(err));
+		goto free_out;
+	}
+
+	if ((err = genl_register_family(&ops)) < 0) {
+		g_critical("Unable to register Generic Netlink family");
+		goto err_out;
+	}
+
+	if ((err = genl_ops_resolve(sock, &ops)) < 0) {
+		g_critical("Unable to resolve family name");
+		goto err_out;
+	}
+
+	/* register notification handler callback */
+	if ((err = nl_socket_modify_cb(sock, NL_CB_VALID, NL_CB_CUSTOM,
+			parse_cb, NULL)) < 0) {
+		g_critical("Unable to modify valid message callback");
+		goto err_out;
+	}
+
+	grp = nl_get_multicast_id(sock, ops.o_id, "notify");
+	nl_socket_add_membership(sock, grp);
+
+	if (!(socket_cb = nl_socket_get_cb(sock))) {
+		g_warning("Can't overwrite recv callback");
+	} else {
+		nl_cb_overwrite_recv(socket_cb, nl_receive_timeout);
+		nl_cb_put(socket_cb);
+	}
+
+	err = send_start(sock, ops.o_id, interface_id);
+	if (err)
+		goto err_out;
+
+	nl_socket_disable_seq_check(sock);
+
+	g_debug("DisplayPort AUX monitor running on interface %u", interface_id);
+
+	while(run_loop == TRUE) {
+		if ((err = nl_recvmsgs_default(sock)) < 0)
+			g_warning("Unable to receive message: %s", nl_geterror(err));
+	}
+
+	send_stop(sock, ops.o_id, interface_id);
+
+err_out:
+	nl_close(sock);
+free_out:
+	nl_socket_free(sock);
+close_out:
+	fclose(pcap_fp);
+}
+
+int main(int argc, char *argv[])
+{
+	int option_idx = 0;
+	int result;
+	unsigned int interface_id = 0;
+	int ret = EXIT_FAILURE;
+	extcap_parameters* extcap_conf = g_new0(extcap_parameters, 1);
+	char* help_header = NULL;
+
+	extcap_base_set_util_info(extcap_conf, argv[0], DPAUXMON_VERSION_MAJOR, DPAUXMON_VERSION_MINOR, DPAUXMON_VERSION_RELEASE,
+		NULL);
+	extcap_base_register_interface(extcap_conf, DPAUXMON_EXTCAP_INTERFACE, "DisplayPort AUX channel monitor capture", 275, "DisplayPort AUX channel monitor");
+
+	help_header = g_strdup_printf(
+		" %s --extcap-interfaces\n"
+		" %s --extcap-interface=%s --extcap-dlts\n"
+		" %s --extcap-interface=%s --extcap-config\n"
+		" %s --extcap-interface=%s --interface_id 0 --fifo myfifo --capture",
+		argv[0], argv[0], DPAUXMON_EXTCAP_INTERFACE, argv[0], DPAUXMON_EXTCAP_INTERFACE, argv[0], DPAUXMON_EXTCAP_INTERFACE);
+	extcap_help_add_header(extcap_conf, help_header);
+	g_free(help_header);
+	extcap_help_add_option(extcap_conf, "--help", "print this help");
+	extcap_help_add_option(extcap_conf, "--version", "print the version");
+	extcap_help_add_option(extcap_conf, "--port <port> ", "the dpauxmon interface index");
+
+	opterr = 0;
+	optind = 0;
+
+	if (argc == 1) {
+		extcap_help_print(extcap_conf);
+		goto end;
+	}
+
+	while ((result = getopt_long(argc, argv, ":", longopts, &option_idx)) != -1) {
+		switch (result) {
+
+		case OPT_HELP:
+			extcap_help_print(extcap_conf);
+			ret = EXIT_SUCCESS;
+			goto end;
+
+		case OPT_VERSION:
+			printf("%s\n", extcap_conf->version);
+			goto end;
+
+		case OPT_INTERFACE_ID:
+			if (!ws_strtou32(optarg, NULL, &interface_id)) {
+				g_warning("Invalid interface id: %s", optarg);
+				goto end;
+			}
+			break;
+
+		case ':':
+			/* missing option argument */
+			g_warning("Option '%s' requires an argument", argv[optind - 1]);
+			break;
+
+		default:
+			if (!extcap_base_parse_options(extcap_conf, result - EXTCAP_OPT_LIST_INTERFACES, optarg)) {
+				g_warning("Invalid option: %s", argv[optind - 1]);
+				goto end;
+			}
+		}
+	}
+
+	extcap_cmdline_debug(argv, argc);
+
+	if (optind != argc) {
+		g_warning("Unexpected extra option: %s", argv[optind]);
+		goto end;
+	}
+
+	if (extcap_base_handle_interface(extcap_conf)) {
+		ret = EXIT_SUCCESS;
+		goto end;
+	}
+
+	if (extcap_conf->show_config) {
+		ret = list_config(extcap_conf->interface);
+		goto end;
+	}
+
+	if (extcap_conf->capture)
+		run_listener(extcap_conf->fifo, interface_id);
+
+end:
+	/* clean up stuff */
+	extcap_base_cleanup(&extcap_conf);
+	return ret;
+}