18c2ecf20Sopenharmony_ci// SPDX-License-Identifier: GPL-2.0+
28c2ecf20Sopenharmony_ci/*
38c2ecf20Sopenharmony_ci * Copyright (C) 2003-2008 Takahiro Hirofuchi
48c2ecf20Sopenharmony_ci */
58c2ecf20Sopenharmony_ci
68c2ecf20Sopenharmony_ci#include <linux/kthread.h>
78c2ecf20Sopenharmony_ci#include <linux/slab.h>
88c2ecf20Sopenharmony_ci#include <linux/scatterlist.h>
98c2ecf20Sopenharmony_ci
108c2ecf20Sopenharmony_ci#include "usbip_common.h"
118c2ecf20Sopenharmony_ci#include "vhci.h"
128c2ecf20Sopenharmony_ci
138c2ecf20Sopenharmony_cistatic void setup_cmd_submit_pdu(struct usbip_header *pdup,  struct urb *urb)
148c2ecf20Sopenharmony_ci{
158c2ecf20Sopenharmony_ci	struct vhci_priv *priv = ((struct vhci_priv *)urb->hcpriv);
168c2ecf20Sopenharmony_ci	struct vhci_device *vdev = priv->vdev;
178c2ecf20Sopenharmony_ci
188c2ecf20Sopenharmony_ci	usbip_dbg_vhci_tx("URB, local devnum %u, remote devid %u\n",
198c2ecf20Sopenharmony_ci			  usb_pipedevice(urb->pipe), vdev->devid);
208c2ecf20Sopenharmony_ci
218c2ecf20Sopenharmony_ci	pdup->base.command   = USBIP_CMD_SUBMIT;
228c2ecf20Sopenharmony_ci	pdup->base.seqnum    = priv->seqnum;
238c2ecf20Sopenharmony_ci	pdup->base.devid     = vdev->devid;
248c2ecf20Sopenharmony_ci	pdup->base.direction = usb_pipein(urb->pipe) ?
258c2ecf20Sopenharmony_ci		USBIP_DIR_IN : USBIP_DIR_OUT;
268c2ecf20Sopenharmony_ci	pdup->base.ep	     = usb_pipeendpoint(urb->pipe);
278c2ecf20Sopenharmony_ci
288c2ecf20Sopenharmony_ci	usbip_pack_pdu(pdup, urb, USBIP_CMD_SUBMIT, 1);
298c2ecf20Sopenharmony_ci
308c2ecf20Sopenharmony_ci	if (urb->setup_packet)
318c2ecf20Sopenharmony_ci		memcpy(pdup->u.cmd_submit.setup, urb->setup_packet, 8);
328c2ecf20Sopenharmony_ci}
338c2ecf20Sopenharmony_ci
348c2ecf20Sopenharmony_cistatic struct vhci_priv *dequeue_from_priv_tx(struct vhci_device *vdev)
358c2ecf20Sopenharmony_ci{
368c2ecf20Sopenharmony_ci	struct vhci_priv *priv, *tmp;
378c2ecf20Sopenharmony_ci	unsigned long flags;
388c2ecf20Sopenharmony_ci
398c2ecf20Sopenharmony_ci	spin_lock_irqsave(&vdev->priv_lock, flags);
408c2ecf20Sopenharmony_ci
418c2ecf20Sopenharmony_ci	list_for_each_entry_safe(priv, tmp, &vdev->priv_tx, list) {
428c2ecf20Sopenharmony_ci		list_move_tail(&priv->list, &vdev->priv_rx);
438c2ecf20Sopenharmony_ci		spin_unlock_irqrestore(&vdev->priv_lock, flags);
448c2ecf20Sopenharmony_ci		return priv;
458c2ecf20Sopenharmony_ci	}
468c2ecf20Sopenharmony_ci
478c2ecf20Sopenharmony_ci	spin_unlock_irqrestore(&vdev->priv_lock, flags);
488c2ecf20Sopenharmony_ci
498c2ecf20Sopenharmony_ci	return NULL;
508c2ecf20Sopenharmony_ci}
518c2ecf20Sopenharmony_ci
528c2ecf20Sopenharmony_cistatic int vhci_send_cmd_submit(struct vhci_device *vdev)
538c2ecf20Sopenharmony_ci{
548c2ecf20Sopenharmony_ci	struct usbip_iso_packet_descriptor *iso_buffer = NULL;
558c2ecf20Sopenharmony_ci	struct vhci_priv *priv = NULL;
568c2ecf20Sopenharmony_ci	struct scatterlist *sg;
578c2ecf20Sopenharmony_ci
588c2ecf20Sopenharmony_ci	struct msghdr msg;
598c2ecf20Sopenharmony_ci	struct kvec *iov;
608c2ecf20Sopenharmony_ci	size_t txsize;
618c2ecf20Sopenharmony_ci
628c2ecf20Sopenharmony_ci	size_t total_size = 0;
638c2ecf20Sopenharmony_ci	int iovnum;
648c2ecf20Sopenharmony_ci	int err = -ENOMEM;
658c2ecf20Sopenharmony_ci	int i;
668c2ecf20Sopenharmony_ci
678c2ecf20Sopenharmony_ci	while ((priv = dequeue_from_priv_tx(vdev)) != NULL) {
688c2ecf20Sopenharmony_ci		int ret;
698c2ecf20Sopenharmony_ci		struct urb *urb = priv->urb;
708c2ecf20Sopenharmony_ci		struct usbip_header pdu_header;
718c2ecf20Sopenharmony_ci
728c2ecf20Sopenharmony_ci		txsize = 0;
738c2ecf20Sopenharmony_ci		memset(&pdu_header, 0, sizeof(pdu_header));
748c2ecf20Sopenharmony_ci		memset(&msg, 0, sizeof(msg));
758c2ecf20Sopenharmony_ci		memset(&iov, 0, sizeof(iov));
768c2ecf20Sopenharmony_ci
778c2ecf20Sopenharmony_ci		usbip_dbg_vhci_tx("setup txdata urb seqnum %lu\n",
788c2ecf20Sopenharmony_ci				  priv->seqnum);
798c2ecf20Sopenharmony_ci
808c2ecf20Sopenharmony_ci		if (urb->num_sgs && usb_pipeout(urb->pipe))
818c2ecf20Sopenharmony_ci			iovnum = 2 + urb->num_sgs;
828c2ecf20Sopenharmony_ci		else
838c2ecf20Sopenharmony_ci			iovnum = 3;
848c2ecf20Sopenharmony_ci
858c2ecf20Sopenharmony_ci		iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
868c2ecf20Sopenharmony_ci		if (!iov) {
878c2ecf20Sopenharmony_ci			usbip_event_add(&vdev->ud, SDEV_EVENT_ERROR_MALLOC);
888c2ecf20Sopenharmony_ci			return -ENOMEM;
898c2ecf20Sopenharmony_ci		}
908c2ecf20Sopenharmony_ci
918c2ecf20Sopenharmony_ci		if (urb->num_sgs)
928c2ecf20Sopenharmony_ci			urb->transfer_flags |= URB_DMA_MAP_SG;
938c2ecf20Sopenharmony_ci
948c2ecf20Sopenharmony_ci		/* 1. setup usbip_header */
958c2ecf20Sopenharmony_ci		setup_cmd_submit_pdu(&pdu_header, urb);
968c2ecf20Sopenharmony_ci		usbip_header_correct_endian(&pdu_header, 1);
978c2ecf20Sopenharmony_ci		iovnum = 0;
988c2ecf20Sopenharmony_ci
998c2ecf20Sopenharmony_ci		iov[iovnum].iov_base = &pdu_header;
1008c2ecf20Sopenharmony_ci		iov[iovnum].iov_len  = sizeof(pdu_header);
1018c2ecf20Sopenharmony_ci		txsize += sizeof(pdu_header);
1028c2ecf20Sopenharmony_ci		iovnum++;
1038c2ecf20Sopenharmony_ci
1048c2ecf20Sopenharmony_ci		/* 2. setup transfer buffer */
1058c2ecf20Sopenharmony_ci		if (!usb_pipein(urb->pipe) && urb->transfer_buffer_length > 0) {
1068c2ecf20Sopenharmony_ci			if (urb->num_sgs &&
1078c2ecf20Sopenharmony_ci				      !usb_endpoint_xfer_isoc(&urb->ep->desc)) {
1088c2ecf20Sopenharmony_ci				for_each_sg(urb->sg, sg, urb->num_sgs, i) {
1098c2ecf20Sopenharmony_ci					iov[iovnum].iov_base = sg_virt(sg);
1108c2ecf20Sopenharmony_ci					iov[iovnum].iov_len = sg->length;
1118c2ecf20Sopenharmony_ci					iovnum++;
1128c2ecf20Sopenharmony_ci				}
1138c2ecf20Sopenharmony_ci			} else {
1148c2ecf20Sopenharmony_ci				iov[iovnum].iov_base = urb->transfer_buffer;
1158c2ecf20Sopenharmony_ci				iov[iovnum].iov_len  =
1168c2ecf20Sopenharmony_ci						urb->transfer_buffer_length;
1178c2ecf20Sopenharmony_ci				iovnum++;
1188c2ecf20Sopenharmony_ci			}
1198c2ecf20Sopenharmony_ci			txsize += urb->transfer_buffer_length;
1208c2ecf20Sopenharmony_ci		}
1218c2ecf20Sopenharmony_ci
1228c2ecf20Sopenharmony_ci		/* 3. setup iso_packet_descriptor */
1238c2ecf20Sopenharmony_ci		if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
1248c2ecf20Sopenharmony_ci			ssize_t len = 0;
1258c2ecf20Sopenharmony_ci
1268c2ecf20Sopenharmony_ci			iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
1278c2ecf20Sopenharmony_ci			if (!iso_buffer) {
1288c2ecf20Sopenharmony_ci				usbip_event_add(&vdev->ud,
1298c2ecf20Sopenharmony_ci						SDEV_EVENT_ERROR_MALLOC);
1308c2ecf20Sopenharmony_ci				goto err_iso_buffer;
1318c2ecf20Sopenharmony_ci			}
1328c2ecf20Sopenharmony_ci
1338c2ecf20Sopenharmony_ci			iov[iovnum].iov_base = iso_buffer;
1348c2ecf20Sopenharmony_ci			iov[iovnum].iov_len  = len;
1358c2ecf20Sopenharmony_ci			iovnum++;
1368c2ecf20Sopenharmony_ci			txsize += len;
1378c2ecf20Sopenharmony_ci		}
1388c2ecf20Sopenharmony_ci
1398c2ecf20Sopenharmony_ci		ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, iov, iovnum,
1408c2ecf20Sopenharmony_ci				     txsize);
1418c2ecf20Sopenharmony_ci		if (ret != txsize) {
1428c2ecf20Sopenharmony_ci			pr_err("sendmsg failed!, ret=%d for %zd\n", ret,
1438c2ecf20Sopenharmony_ci			       txsize);
1448c2ecf20Sopenharmony_ci			usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP);
1458c2ecf20Sopenharmony_ci			err = -EPIPE;
1468c2ecf20Sopenharmony_ci			goto err_tx;
1478c2ecf20Sopenharmony_ci		}
1488c2ecf20Sopenharmony_ci
1498c2ecf20Sopenharmony_ci		kfree(iov);
1508c2ecf20Sopenharmony_ci		/* This is only for isochronous case */
1518c2ecf20Sopenharmony_ci		kfree(iso_buffer);
1528c2ecf20Sopenharmony_ci		iso_buffer = NULL;
1538c2ecf20Sopenharmony_ci
1548c2ecf20Sopenharmony_ci		usbip_dbg_vhci_tx("send txdata\n");
1558c2ecf20Sopenharmony_ci
1568c2ecf20Sopenharmony_ci		total_size += txsize;
1578c2ecf20Sopenharmony_ci	}
1588c2ecf20Sopenharmony_ci
1598c2ecf20Sopenharmony_ci	return total_size;
1608c2ecf20Sopenharmony_ci
1618c2ecf20Sopenharmony_cierr_tx:
1628c2ecf20Sopenharmony_ci	kfree(iso_buffer);
1638c2ecf20Sopenharmony_cierr_iso_buffer:
1648c2ecf20Sopenharmony_ci	kfree(iov);
1658c2ecf20Sopenharmony_ci
1668c2ecf20Sopenharmony_ci	return err;
1678c2ecf20Sopenharmony_ci}
1688c2ecf20Sopenharmony_ci
1698c2ecf20Sopenharmony_cistatic struct vhci_unlink *dequeue_from_unlink_tx(struct vhci_device *vdev)
1708c2ecf20Sopenharmony_ci{
1718c2ecf20Sopenharmony_ci	struct vhci_unlink *unlink, *tmp;
1728c2ecf20Sopenharmony_ci	unsigned long flags;
1738c2ecf20Sopenharmony_ci
1748c2ecf20Sopenharmony_ci	spin_lock_irqsave(&vdev->priv_lock, flags);
1758c2ecf20Sopenharmony_ci
1768c2ecf20Sopenharmony_ci	list_for_each_entry_safe(unlink, tmp, &vdev->unlink_tx, list) {
1778c2ecf20Sopenharmony_ci		list_move_tail(&unlink->list, &vdev->unlink_rx);
1788c2ecf20Sopenharmony_ci		spin_unlock_irqrestore(&vdev->priv_lock, flags);
1798c2ecf20Sopenharmony_ci		return unlink;
1808c2ecf20Sopenharmony_ci	}
1818c2ecf20Sopenharmony_ci
1828c2ecf20Sopenharmony_ci	spin_unlock_irqrestore(&vdev->priv_lock, flags);
1838c2ecf20Sopenharmony_ci
1848c2ecf20Sopenharmony_ci	return NULL;
1858c2ecf20Sopenharmony_ci}
1868c2ecf20Sopenharmony_ci
1878c2ecf20Sopenharmony_cistatic int vhci_send_cmd_unlink(struct vhci_device *vdev)
1888c2ecf20Sopenharmony_ci{
1898c2ecf20Sopenharmony_ci	struct vhci_unlink *unlink = NULL;
1908c2ecf20Sopenharmony_ci
1918c2ecf20Sopenharmony_ci	struct msghdr msg;
1928c2ecf20Sopenharmony_ci	struct kvec iov;
1938c2ecf20Sopenharmony_ci	size_t txsize;
1948c2ecf20Sopenharmony_ci	size_t total_size = 0;
1958c2ecf20Sopenharmony_ci
1968c2ecf20Sopenharmony_ci	while ((unlink = dequeue_from_unlink_tx(vdev)) != NULL) {
1978c2ecf20Sopenharmony_ci		int ret;
1988c2ecf20Sopenharmony_ci		struct usbip_header pdu_header;
1998c2ecf20Sopenharmony_ci
2008c2ecf20Sopenharmony_ci		memset(&pdu_header, 0, sizeof(pdu_header));
2018c2ecf20Sopenharmony_ci		memset(&msg, 0, sizeof(msg));
2028c2ecf20Sopenharmony_ci		memset(&iov, 0, sizeof(iov));
2038c2ecf20Sopenharmony_ci
2048c2ecf20Sopenharmony_ci		usbip_dbg_vhci_tx("setup cmd unlink, %lu\n", unlink->seqnum);
2058c2ecf20Sopenharmony_ci
2068c2ecf20Sopenharmony_ci		/* 1. setup usbip_header */
2078c2ecf20Sopenharmony_ci		pdu_header.base.command = USBIP_CMD_UNLINK;
2088c2ecf20Sopenharmony_ci		pdu_header.base.seqnum  = unlink->seqnum;
2098c2ecf20Sopenharmony_ci		pdu_header.base.devid	= vdev->devid;
2108c2ecf20Sopenharmony_ci		pdu_header.base.ep	= 0;
2118c2ecf20Sopenharmony_ci		pdu_header.u.cmd_unlink.seqnum = unlink->unlink_seqnum;
2128c2ecf20Sopenharmony_ci
2138c2ecf20Sopenharmony_ci		usbip_header_correct_endian(&pdu_header, 1);
2148c2ecf20Sopenharmony_ci
2158c2ecf20Sopenharmony_ci		iov.iov_base = &pdu_header;
2168c2ecf20Sopenharmony_ci		iov.iov_len  = sizeof(pdu_header);
2178c2ecf20Sopenharmony_ci		txsize = sizeof(pdu_header);
2188c2ecf20Sopenharmony_ci
2198c2ecf20Sopenharmony_ci		ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, &iov, 1, txsize);
2208c2ecf20Sopenharmony_ci		if (ret != txsize) {
2218c2ecf20Sopenharmony_ci			pr_err("sendmsg failed!, ret=%d for %zd\n", ret,
2228c2ecf20Sopenharmony_ci			       txsize);
2238c2ecf20Sopenharmony_ci			usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP);
2248c2ecf20Sopenharmony_ci			return -1;
2258c2ecf20Sopenharmony_ci		}
2268c2ecf20Sopenharmony_ci
2278c2ecf20Sopenharmony_ci		usbip_dbg_vhci_tx("send txdata\n");
2288c2ecf20Sopenharmony_ci
2298c2ecf20Sopenharmony_ci		total_size += txsize;
2308c2ecf20Sopenharmony_ci	}
2318c2ecf20Sopenharmony_ci
2328c2ecf20Sopenharmony_ci	return total_size;
2338c2ecf20Sopenharmony_ci}
2348c2ecf20Sopenharmony_ci
2358c2ecf20Sopenharmony_ciint vhci_tx_loop(void *data)
2368c2ecf20Sopenharmony_ci{
2378c2ecf20Sopenharmony_ci	struct usbip_device *ud = data;
2388c2ecf20Sopenharmony_ci	struct vhci_device *vdev = container_of(ud, struct vhci_device, ud);
2398c2ecf20Sopenharmony_ci
2408c2ecf20Sopenharmony_ci	while (!kthread_should_stop()) {
2418c2ecf20Sopenharmony_ci		if (vhci_send_cmd_submit(vdev) < 0)
2428c2ecf20Sopenharmony_ci			break;
2438c2ecf20Sopenharmony_ci
2448c2ecf20Sopenharmony_ci		if (vhci_send_cmd_unlink(vdev) < 0)
2458c2ecf20Sopenharmony_ci			break;
2468c2ecf20Sopenharmony_ci
2478c2ecf20Sopenharmony_ci		wait_event_interruptible(vdev->waitq_tx,
2488c2ecf20Sopenharmony_ci					 (!list_empty(&vdev->priv_tx) ||
2498c2ecf20Sopenharmony_ci					  !list_empty(&vdev->unlink_tx) ||
2508c2ecf20Sopenharmony_ci					  kthread_should_stop()));
2518c2ecf20Sopenharmony_ci
2528c2ecf20Sopenharmony_ci		usbip_dbg_vhci_tx("pending urbs ?, now wake up\n");
2538c2ecf20Sopenharmony_ci	}
2548c2ecf20Sopenharmony_ci
2558c2ecf20Sopenharmony_ci	return 0;
2568c2ecf20Sopenharmony_ci}
257