Implemented link layer using kernel sockets. Link layer
manages network communication and provides interface for upper
layers of MA-USB stack.
Signed-off-by: Vladimir Stankovic <vladimir.stankovic@xxxxxxxxxxxxxxx>
---
drivers/usb/mausb_host/Makefile | 1 +
drivers/usb/mausb_host/ip_link.c | 374 +++++++++++++++++++++++++++++++
drivers/usb/mausb_host/ip_link.h | 87 +++++++
3 files changed, 462 insertions(+)
create mode 100644 drivers/usb/mausb_host/ip_link.c
create mode 100644 drivers/usb/mausb_host/ip_link.h
diff --git a/drivers/usb/mausb_host/Makefile b/drivers/usb/mausb_host/Makefile
index 2e353fa0958b..19445b73b50b 100644
--- a/drivers/usb/mausb_host/Makefile
+++ b/drivers/usb/mausb_host/Makefile
@@ -8,5 +8,6 @@
obj-$(CONFIG_HOST_MAUSB) += mausb_host.o
mausb_host-y := mausb_core.o
mausb_host-y += utils.o
+mausb_host-y += ip_link.o
ccflags-y += -I$(srctree)/$(src)
diff --git a/drivers/usb/mausb_host/ip_link.c b/drivers/usb/mausb_host/ip_link.c
new file mode 100644
index 000000000000..49b592c02210
--- /dev/null
+++ b/drivers/usb/mausb_host/ip_link.c
@@ -0,0 +1,374 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2019 - 2020 DisplayLink (UK) Ltd.
+ */
+#include "ip_link.h"
+
+#include <linux/in.h>
+#include <linux/inet.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/net.h>
+#include <linux/skbuff.h>
+#include <linux/socket.h>
+#include <linux/workqueue.h>
+#include <net/sock.h>
+#include <net/tcp.h>
+
+#include "utils.h"
+
+static void __mausb_ip_connect(struct work_struct *work);
+static int __mausb_ip_recv(struct mausb_ip_ctx *ip_ctx);
+static void __mausb_ip_recv_work(struct work_struct *work);
+static inline void __mausb_ip_recv_ctx_clear(struct mausb_ip_recv_ctx
+ *recv_ctx);
+static inline void __mausb_ip_recv_ctx_free(struct mausb_ip_recv_ctx
+ *recv_ctx);
+
+int mausb_init_ip_ctx(struct mausb_ip_ctx **ip_ctx,
+ struct net *net_ns,
+ char ip_addr[INET6_ADDRSTRLEN],
+ u16 port, void *context,
+ void (*fn_callback)(void *ctx, enum mausb_channel channel,
+ enum mausb_link_action act,
+ int status, void *data),
+ enum mausb_channel channel)
+{
+ struct mausb_ip_ctx *ctx;
+
+ *ip_ctx = kzalloc(sizeof(**ip_ctx), GFP_ATOMIC);
+ if (!*ip_ctx)
+ return -ENOMEM;
+
+ ctx = *ip_ctx;
+ ctx->client_socket = NULL;
+ __mausb_ip_recv_ctx_clear(&ctx->recv_ctx);
+
+ if (in4_pton(ip_addr, -1,
+ (u8 *)&ctx->dev_addr_in.sa_in.sin_addr.s_addr, -1,
+ NULL) == 1) {
+ ctx->dev_addr_in.sa_in.sin_family = AF_INET;
+ ctx->dev_addr_in.sa_in.sin_port = htons(port);
+#if IS_ENABLED(CONFIG_IPV6)
+ } else if (in6_pton(ip_addr, -1,
+ (u8 *)&ctx->dev_addr_in.sa_in6.sin6_addr.in6_u, -1,
+ NULL) == 1) {
+ ctx->dev_addr_in.sa_in6.sin6_family = AF_INET6;
+ ctx->dev_addr_in.sa_in6.sin6_port = htons(port);
+#endif
+ } else {
+ mausb_pr_err("Invalid IP address received: address=%s",
+ ip_addr);
+ kfree(ctx);
+ return -EINVAL;
+ }
+
+ ctx->net_ns = net_ns;
+
+ if (channel == MAUSB_ISOCH_CHANNEL)
+ ctx->udp = true;
+
+ ctx->connect_workq = alloc_ordered_workqueue("connect_workq",
+ WQ_MEM_RECLAIM);
+ if (!ctx->connect_workq) {
+ kfree(ctx);
+ return -ENOMEM;
+ }
+
+ ctx->recv_workq = alloc_ordered_workqueue("recv_workq", WQ_MEM_RECLAIM);
+ if (!ctx->recv_workq) {
+ destroy_workqueue(ctx->connect_workq);
+ kfree(ctx);
+ return -ENOMEM;
+ }
+
+ INIT_WORK(&ctx->connect_work, __mausb_ip_connect);
+ INIT_WORK(&ctx->recv_work, __mausb_ip_recv_work);
+
+ ctx->channel = channel;
+ ctx->ctx = context;
+ ctx->fn_callback = fn_callback;
+
+ return 0;
+}
+
+void mausb_destroy_ip_ctx(struct mausb_ip_ctx *ip_ctx)
+{
+ if (!ip_ctx)
+ return;
+
+ if (ip_ctx->connect_workq) {
+ flush_workqueue(ip_ctx->connect_workq);
+ destroy_workqueue(ip_ctx->connect_workq);
+ }
+
+ if (ip_ctx->recv_workq) {
+ flush_workqueue(ip_ctx->recv_workq);
+ destroy_workqueue(ip_ctx->recv_workq);
+ }
+ if (ip_ctx->client_socket)
+ sock_release(ip_ctx->client_socket);
+ __mausb_ip_recv_ctx_free(&ip_ctx->recv_ctx);
+
+ kfree(ip_ctx);
+}
+
+static void __mausb_ip_set_options(struct socket *sock, bool udp)
+{
+ u32 optval = 0;
+ unsigned int optlen = sizeof(optval);
+ int status = 0;
+ struct __kernel_sock_timeval timeo = {.tv_sec = 0, .tv_usec = 500000U };
+ struct __kernel_sock_timeval send_timeo = {.tv_sec = 1, .tv_usec = 0 };
+
+ if (!udp) {
+ optval = 1;
+ status = kernel_setsockopt(sock, IPPROTO_TCP, TCP_NODELAY,
+ (char *)&optval, optlen);
+ if (status < 0)
+ mausb_pr_warn("Failed to set tcp no delay option: status=%d",
+ status);
+ }
+
+ status = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO_NEW,
+ (char *)&timeo, sizeof(timeo));
+ if (status < 0)
+ mausb_pr_warn("Failed to set recv timeout option: status=%d",
+ status);
+
+ status = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDTIMEO_NEW,
+ (char *)&send_timeo, sizeof(send_timeo));
+ if (status < 0)
+ mausb_pr_warn("Failed to set snd timeout option: status=%d",
+ status);
+
+ optval = MAUSB_LINK_BUFF_SIZE;
+ status = kernel_setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
+ (char *)&optval, optlen);
+ if (status < 0)
+ mausb_pr_warn("Failed to set recv buffer size: status=%d",
+ status);
+
+ optval = MAUSB_LINK_BUFF_SIZE;
+ status = kernel_setsockopt(sock, SOL_SOCKET, SO_SNDBUF,
+ (char *)&optval, optlen);
+ if (status < 0)
+ mausb_pr_warn("Failed to set send buffer size: status=%d",
+ status);
+
+ optval = MAUSB_LINK_TOS_LEVEL_EF;
+ status = kernel_setsockopt(sock, IPPROTO_IP, IP_TOS,
+ (char *)&optval, optlen);
+ if (status < 0)
+ mausb_pr_warn("Failed to set QOS: status=%d", status);
+}
+
+static void __mausb_ip_connect(struct work_struct *work)
+{
+ int status = 0;
+ struct sockaddr *sa;
+ int sa_size;
+ struct mausb_ip_ctx *ip_ctx = container_of(work, struct mausb_ip_ctx,
+ connect_work);
+ unsigned short int family = ip_ctx->dev_addr_in.sa_in.sin_family;
+
+ if (!ip_ctx->udp) {
+ status = sock_create_kern(ip_ctx->net_ns, family, SOCK_STREAM,
+ IPPROTO_TCP, &ip_ctx->client_socket);
+ if (status < 0) {
+ mausb_pr_err("Failed to create socket: status=%d",
+ status);
+ goto callback;
+ }
+ } else {
+ status = sock_create_kern(ip_ctx->net_ns, family, SOCK_DGRAM,
+ IPPROTO_UDP, &ip_ctx->client_socket);
+ if (status < 0) {
+ mausb_pr_err("Failed to create socket: status=%d",
+ status);
+ goto callback;
+ }
+ }
+
+ __mausb_ip_set_options((struct socket *)ip_ctx->client_socket,
+ ip_ctx->udp);
+
+ if (family == AF_INET) {
+ sa = (struct sockaddr *)&ip_ctx->dev_addr_in.sa_in;
+ sa_size = sizeof(ip_ctx->dev_addr_in.sa_in);
+ mausb_pr_info("Connecting to %pI4:%d, status=%d",
+ &ip_ctx->dev_addr_in.sa_in.sin_addr,
+ htons(ip_ctx->dev_addr_in.sa_in.sin_port),
+ status);
+#if IS_ENABLED(CONFIG_IPV6)
+ } else if (family == AF_INET6) {
+ sa = (struct sockaddr *)&ip_ctx->dev_addr_in.sa_in6;
+ sa_size = sizeof(ip_ctx->dev_addr_in.sa_in6);
+ mausb_pr_info("Connecting to %pI6c:%d, status=%d",
+ &ip_ctx->dev_addr_in.sa_in6.sin6_addr,
+ htons(ip_ctx->dev_addr_in.sa_in6.sin6_port),
+ status);
+#endif
+ } else {
+ mausb_pr_err("Wrong network family provided");
+ status = -EINVAL;
+ goto callback;
+ }
+
+ status = kernel_connect(ip_ctx->client_socket, sa, sa_size, O_RDWR);
+ if (status < 0) {
+ mausb_pr_err("Failed to connect to host, status=%d", status);
+ goto clear_socket;
+ }
+
+ queue_work(ip_ctx->recv_workq, &ip_ctx->recv_work);
+
+ goto callback;
+
+clear_socket:
+ sock_release(ip_ctx->client_socket);
+ ip_ctx->client_socket = NULL;
+callback:
+ ip_ctx->fn_callback(ip_ctx->ctx, ip_ctx->channel, MAUSB_LINK_CONNECT,
+ status, NULL);
+}
+
+void mausb_ip_connect_async(struct mausb_ip_ctx *ip_ctx)
+{
+ queue_work(ip_ctx->connect_workq, &ip_ctx->connect_work);
+}
+
+int mausb_ip_disconnect(struct mausb_ip_ctx *ip_ctx)
+{
+ if (ip_ctx && ip_ctx->client_socket)
+ return kernel_sock_shutdown(ip_ctx->client_socket, SHUT_RDWR);
+ return 0;
+}
+
+int mausb_ip_send(struct mausb_ip_ctx *ip_ctx,
+ struct mausb_kvec_data_wrapper *wrapper)
+{
+ struct msghdr msghd;
+
+ if (!ip_ctx) {
+ mausb_pr_alert("Socket ctx is NULL!");
+ return -EINVAL;
+ }
+
+ memset(&msghd, 0, sizeof(msghd));
+ msghd.msg_flags = MSG_WAITALL;
+
+ return kernel_sendmsg(ip_ctx->client_socket, &msghd, wrapper->kvec,
+ wrapper->kvec_num, wrapper->length);
+}
+
+static inline void __mausb_ip_recv_ctx_clear(struct mausb_ip_recv_ctx *recv_ctx)
+{
+ recv_ctx->buffer = NULL;
+ recv_ctx->left = 0;
+ recv_ctx->received = 0;
+}
+
+static inline void __mausb_ip_recv_ctx_free(struct mausb_ip_recv_ctx *recv_ctx)
+{
+ kfree(recv_ctx->buffer);
+ __mausb_ip_recv_ctx_clear(recv_ctx);
+}
+
+static int __mausb_ip_recv(struct mausb_ip_ctx *ip_ctx)
+{
+ struct msghdr msghd;
+ struct kvec vec;
+ int status;
+ bool peek = true;
+ unsigned int optval = 1;
+ struct socket *client_socket = (struct socket *)ip_ctx->client_socket;
+
+ /* receive with timeout of 0.5s */
+ while (true) {
+ memset(&msghd, 0, sizeof(msghd));
+ if (peek) {
+ vec.iov_base = ip_ctx->recv_ctx.common_hdr;
+ vec.iov_len = sizeof(ip_ctx->recv_ctx.common_hdr);
+ msghd.msg_flags = MSG_PEEK;
+ } else {
+ vec.iov_base =
+ ip_ctx->recv_ctx.buffer +
+ ip_ctx->recv_ctx.received;
+ vec.iov_len = ip_ctx->recv_ctx.left;
+ msghd.msg_flags = MSG_WAITALL;
+ }
+
+ if (!ip_ctx->udp) {
+ status = kernel_setsockopt(client_socket, IPPROTO_TCP,
+ TCP_QUICKACK,
+ (char *)&optval,
+ sizeof(optval));
+ if (status != 0) {
+ mausb_pr_warn("Setting TCP_QUICKACK failed, status=%d",
+ status);
+ }
+ }
+
+ status = kernel_recvmsg(client_socket, &msghd, &vec, 1,
+ vec.iov_len, (int)msghd.msg_flags);
+ if (status == -EAGAIN) {
+ return -EAGAIN;
+ } else if (status <= 0) {
+ mausb_pr_warn("kernel_recvmsg, status=%d", status);
+
+ __mausb_ip_recv_ctx_free(&ip_ctx->recv_ctx);
+ ip_ctx->fn_callback(ip_ctx->ctx, ip_ctx->channel,
+ MAUSB_LINK_RECV, status, NULL);
+ return status;
+ }
+
+ mausb_pr_debug("kernel_recvmsg, status=%d", status);
+
+ if (peek) {
+ if ((unsigned int)status <
+ sizeof(ip_ctx->recv_ctx.common_hdr))
+ return -EAGAIN;
+ /* length field of mausb_common_hdr */
+ ip_ctx->recv_ctx.left =
+ *(u16 *)(&ip_ctx->recv_ctx.common_hdr[2]);
+ ip_ctx->recv_ctx.received = 0;
+ ip_ctx->recv_ctx.buffer =
+ kzalloc(ip_ctx->recv_ctx.left, GFP_KERNEL);
+ peek = false;
+ if (!ip_ctx->recv_ctx.buffer) {
+ ip_ctx->fn_callback(ip_ctx->ctx,
+ ip_ctx->channel,
+ MAUSB_LINK_RECV,
+ -ENOMEM, NULL);
+ return -ENOMEM;
+ }
+ } else {
+ if (status < ip_ctx->recv_ctx.left) {
+ ip_ctx->recv_ctx.left -= (u16)status;
+ ip_ctx->recv_ctx.received += (u16)status;
+ } else {
+ ip_ctx->fn_callback(ip_ctx->ctx,
+ ip_ctx->channel,
+ MAUSB_LINK_RECV, status,
+ ip_ctx->recv_ctx.buffer);
+ __mausb_ip_recv_ctx_clear(&ip_ctx->recv_ctx);
+ peek = true;
+ }
+ }
+ }
+
+ return status;
+}
+
+static void __mausb_ip_recv_work(struct work_struct *work)
+{
+ struct mausb_ip_ctx *ip_ctx = container_of(work, struct mausb_ip_ctx,
+ recv_work);
+ int status = __mausb_ip_recv(ip_ctx);
+
+ if (status <= 0 && status != -EAGAIN)
+ return;
+
+ queue_work(ip_ctx->recv_workq, &ip_ctx->recv_work);
+}
diff --git a/drivers/usb/mausb_host/ip_link.h b/drivers/usb/mausb_host/ip_link.h
new file mode 100644
index 000000000000..5946151e4e4e
--- /dev/null
+++ b/drivers/usb/mausb_host/ip_link.h
@@ -0,0 +1,87 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2019 - 2020 DisplayLink (UK) Ltd.
+ */
+#ifndef __MAUSB_IP_LINK_H__
+#define __MAUSB_IP_LINK_H__
+
+#include <linux/inet.h>
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <net/net_namespace.h>
+
+#define MAUSB_LINK_BUFF_SIZE 16777216
+#define MAUSB_LINK_TOS_LEVEL_EF 0xB8
+
+enum mausb_link_action {
+ MAUSB_LINK_CONNECT = 0,
+ MAUSB_LINK_DISCONNECT = 1,
+ MAUSB_LINK_RECV = 2,
+ MAUSB_LINK_SEND = 3
+};
+
+enum mausb_channel {
+ MAUSB_CTRL_CHANNEL = 0,
+ MAUSB_ISOCH_CHANNEL = 1,
+ MAUSB_BULK_CHANNEL = 2,
+ MAUSB_INTR_CHANNEL = 3,
+ MAUSB_MGMT_CHANNEL = 4
+};
+
+struct mausb_kvec_data_wrapper {
+ struct kvec *kvec;
+ u32 kvec_num;
+ u32 length;
+};
+
+struct mausb_ip_recv_ctx {
+ u16 left;
+ u16 received;
+ char *buffer;
+ char common_hdr[12] __aligned(4);
+};
+
+struct mausb_ip_ctx {
+ struct socket *client_socket;
+ union {
+ struct sockaddr_in sa_in;
+#if IS_ENABLED(CONFIG_IPV6)
+ struct sockaddr_in6 sa_in6;
+#endif
+ } dev_addr_in;
+ struct net *net_ns;
+ bool udp;
+
+ /* Queues to schedule rx work */
+ struct workqueue_struct *recv_workq;
+ struct workqueue_struct *connect_workq;
+ struct work_struct recv_work;
+ struct work_struct connect_work;
+
+ struct mausb_ip_recv_ctx recv_ctx; /* recv buffer */
+
+ enum mausb_channel channel;
+ void *ctx;
+ /* callback should store task into hpal queue */
+ void (*fn_callback)(void *ctx, enum mausb_channel channel,
+ enum mausb_link_action act, int status, void *data);
+};
+
+int mausb_init_ip_ctx(struct mausb_ip_ctx **ip_ctx,
+ struct net *net_ns,
+ char ip_addr[INET6_ADDRSTRLEN],
+ u16 port,
+ void *ctx,
+ void (*ctx_callback)(void *ctx,
+ enum mausb_channel channel,
+ enum mausb_link_action act,
+ int status, void *data),
+ enum mausb_channel channel);
+int mausb_ip_disconnect(struct mausb_ip_ctx *ip_ctx);
+int mausb_ip_send(struct mausb_ip_ctx *ip_ctx,
+ struct mausb_kvec_data_wrapper *wrapper);
+
+void mausb_destroy_ip_ctx(struct mausb_ip_ctx *ip_ctx);
+void mausb_ip_connect_async(struct mausb_ip_ctx *ip_ctx);
+
+#endif /* __MAUSB_IP_LINK_H__ */
--
2.17.1
