1 files changed, 303 insertions, 0 deletions
diff --git a/tools/usb/usbip/src/usbip_network.c b/tools/usb/usbip/src/usbip_network.c
new file mode 100644
index 000000000..ed4dc8c14
--- /dev/null
+++ b/tools/usb/usbip/src/usbip_network.c
@@ -0,0 +1,303 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
+ *               2005-2007 Takahiro Hirofuchi
+ */
+
+#include <sys/socket.h>
+
+#include <string.h>
+
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <netinet/tcp.h>
+#include <unistd.h>
+
+#ifdef HAVE_LIBWRAP
+#include <tcpd.h>
+#endif
+
+#include "usbip_common.h"
+#include "usbip_network.h"
+
+int usbip_port = 3240;
+char *usbip_port_string = "3240";
+
+void usbip_setup_port_number(char *arg)
+{
+	dbg("parsing port arg '%s'", arg);
+	char *end;
+	unsigned long int port = strtoul(arg, &end, 10);
+
+	if (end == arg) {
+		err("port: could not parse '%s' as a decimal integer", arg);
+		return;
+	}
+
+	if (*end != '\0') {
+		err("port: garbage at end of '%s'", arg);
+		return;
+	}
+
+	if (port > UINT16_MAX) {
+		err("port: %s too high (max=%d)",
+		    arg, UINT16_MAX);
+		return;
+	}
+
+	usbip_port = port;
+	usbip_port_string = arg;
+	info("using port %d (\"%s\")", usbip_port, usbip_port_string);
+}
+
+uint32_t usbip_net_pack_uint32_t(int pack, uint32_t num)
+{
+	uint32_t i;
+
+	if (pack)
+		i = htonl(num);
+	else
+		i = ntohl(num);
+
+	return i;
+}
+
+uint16_t usbip_net_pack_uint16_t(int pack, uint16_t num)
+{
+	uint16_t i;
+
+	if (pack)
+		i = htons(num);
+	else
+		i = ntohs(num);
+
+	return i;
+}
+
+void usbip_net_pack_usb_device(int pack, struct usbip_usb_device *udev)
+{
+	udev->busnum = usbip_net_pack_uint32_t(pack, udev->busnum);
+	udev->devnum = usbip_net_pack_uint32_t(pack, udev->devnum);
+	udev->speed = usbip_net_pack_uint32_t(pack, udev->speed);
+
+	udev->idVendor = usbip_net_pack_uint16_t(pack, udev->idVendor);
+	udev->idProduct = usbip_net_pack_uint16_t(pack, udev->idProduct);
+	udev->bcdDevice = usbip_net_pack_uint16_t(pack, udev->bcdDevice);
+}
+
+void usbip_net_pack_usb_interface(int pack __attribute__((unused)),
+				  struct usbip_usb_interface *udev
+				  __attribute__((unused)))
+{
+	/* uint8_t members need nothing */
+}
+
+static ssize_t usbip_net_xmit(int sockfd, void *buff, size_t bufflen,
+			      int sending)
+{
+	ssize_t nbytes;
+	ssize_t total = 0;
+
+	if (!bufflen)
+		return 0;
+
+	do {
+		if (sending)
+			nbytes = send(sockfd, buff, bufflen, 0);
+		else
+			nbytes = recv(sockfd, buff, bufflen, MSG_WAITALL);
+
+		if (nbytes <= 0)
+			return -1;
+
+		buff	 = (void *)((intptr_t) buff + nbytes);
+		bufflen	-= nbytes;
+		total	+= nbytes;
+
+	} while (bufflen > 0);
+
+	return total;
+}
+
+ssize_t usbip_net_recv(int sockfd, void *buff, size_t bufflen)
+{
+	return usbip_net_xmit(sockfd, buff, bufflen, 0);
+}
+
+ssize_t usbip_net_send(int sockfd, void *buff, size_t bufflen)
+{
+	return usbip_net_xmit(sockfd, buff, bufflen, 1);
+}
+
+static inline void usbip_net_pack_op_common(int pack,
+					    struct op_common *op_common)
+{
+	op_common->version = usbip_net_pack_uint16_t(pack, op_common->version);
+	op_common->code = usbip_net_pack_uint16_t(pack, op_common->code);
+	op_common->status = usbip_net_pack_uint32_t(pack, op_common->status);
+}
+
+int usbip_net_send_op_common(int sockfd, uint32_t code, uint32_t status)
+{
+	struct op_common op_common;
+	int rc;
+
+	memset(&op_common, 0, sizeof(op_common));
+
+	op_common.version = USBIP_VERSION;
+	op_common.code    = code;
+	op_common.status  = status;
+
+	usbip_net_pack_op_common(1, &op_common);
+
+	rc = usbip_net_send(sockfd, &op_common, sizeof(op_common));
+	if (rc < 0) {
+		dbg("usbip_net_send failed: %d", rc);
+		return -1;
+	}
+
+	return 0;
+}
+
+int usbip_net_recv_op_common(int sockfd, uint16_t *code, int *status)
+{
+	struct op_common op_common;
+	int rc;
+
+	memset(&op_common, 0, sizeof(op_common));
+
+	rc = usbip_net_recv(sockfd, &op_common, sizeof(op_common));
+	if (rc < 0) {
+		dbg("usbip_net_recv failed: %d", rc);
+		goto err;
+	}
+
+	usbip_net_pack_op_common(0, &op_common);
+
+	if (op_common.version != USBIP_VERSION) {
+		err("USBIP Kernel and tool version mismatch: %d %d:",
+		    op_common.version, USBIP_VERSION);
+		goto err;
+	}
+
+	switch (*code) {
+	case OP_UNSPEC:
+		break;
+	default:
+		if (op_common.code != *code) {
+			dbg("unexpected pdu %#0x for %#0x", op_common.code,
+			    *code);
+			/* return error status */
+			*status = ST_ERROR;
+			goto err;
+		}
+	}
+
+	*status = op_common.status;
+
+	if (op_common.status != ST_OK) {
+		dbg("request failed at peer: %d", op_common.status);
+		goto err;
+	}
+
+	*code = op_common.code;
+
+	return 0;
+err:
+	return -1;
+}
+
+int usbip_net_set_reuseaddr(int sockfd)
+{
+	const int val = 1;
+	int ret;
+
+	ret = setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
+	if (ret < 0)
+		dbg("setsockopt: SO_REUSEADDR");
+
+	return ret;
+}
+
+int usbip_net_set_nodelay(int sockfd)
+{
+	const int val = 1;
+	int ret;
+
+	ret = setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val));
+	if (ret < 0)
+		dbg("setsockopt: TCP_NODELAY");
+
+	return ret;
+}
+
+int usbip_net_set_keepalive(int sockfd)
+{
+	const int val = 1;
+	int ret;
+
+	ret = setsockopt(sockfd, SOL_SOCKET, SO_KEEPALIVE, &val, sizeof(val));
+	if (ret < 0)
+		dbg("setsockopt: SO_KEEPALIVE");
+
+	return ret;
+}
+
+int usbip_net_set_v6only(int sockfd)
+{
+	const int val = 1;
+	int ret;
+
+	ret = setsockopt(sockfd, IPPROTO_IPV6, IPV6_V6ONLY, &val, sizeof(val));
+	if (ret < 0)
+		dbg("setsockopt: IPV6_V6ONLY");
+
+	return ret;
+}
+
+/*
+ * IPv6 Ready
+ */
+int usbip_net_tcp_connect(char *hostname, char *service)
+{
+	struct addrinfo hints, *res, *rp;
+	int sockfd;
+	int ret;
+
+	memset(&hints, 0, sizeof(hints));
+	hints.ai_family = AF_UNSPEC;
+	hints.ai_socktype = SOCK_STREAM;
+
+	/* get all possible addresses */
+	ret = getaddrinfo(hostname, service, &hints, &res);
+	if (ret < 0) {
+		dbg("getaddrinfo: %s service %s: %s", hostname, service,
+		    gai_strerror(ret));
+		return ret;
+	}
+
+	/* try the addresses */
+	for (rp = res; rp; rp = rp->ai_next) {
+		sockfd = socket(rp->ai_family, rp->ai_socktype,
+				rp->ai_protocol);
+		if (sockfd < 0)
+			continue;
+
+		/* should set TCP_NODELAY for usbip */
+		usbip_net_set_nodelay(sockfd);
+		/* TODO: write code for heartbeat */
+		usbip_net_set_keepalive(sockfd);
+
+		if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) == 0)
+			break;
+
+		close(sockfd);
+	}
+
+	freeaddrinfo(res);
+
+	if (!rp)
+		return EAI_SYSTEM;
+
+	return sockfd;
+}