18c2ecf20Sopenharmony_ci// SPDX-License-Identifier: GPL-2.0+
28c2ecf20Sopenharmony_ci/*
38c2ecf20Sopenharmony_ci * Copyright (C) 2015 Karol Kosik <karo9@interia.eu>
48c2ecf20Sopenharmony_ci * Copyright (C) 2015-2016 Samsung Electronics
58c2ecf20Sopenharmony_ci *               Igor Kotrasinski <i.kotrasinsk@samsung.com>
68c2ecf20Sopenharmony_ci */
78c2ecf20Sopenharmony_ci
88c2ecf20Sopenharmony_ci#include <net/sock.h>
98c2ecf20Sopenharmony_ci#include <linux/list.h>
108c2ecf20Sopenharmony_ci#include <linux/kthread.h>
118c2ecf20Sopenharmony_ci
128c2ecf20Sopenharmony_ci#include "usbip_common.h"
138c2ecf20Sopenharmony_ci#include "vudc.h"
148c2ecf20Sopenharmony_ci
158c2ecf20Sopenharmony_cistatic inline void setup_base_pdu(struct usbip_header_basic *base,
168c2ecf20Sopenharmony_ci				  __u32 command, __u32 seqnum)
178c2ecf20Sopenharmony_ci{
188c2ecf20Sopenharmony_ci	base->command	= command;
198c2ecf20Sopenharmony_ci	base->seqnum	= seqnum;
208c2ecf20Sopenharmony_ci	base->devid	= 0;
218c2ecf20Sopenharmony_ci	base->ep	= 0;
228c2ecf20Sopenharmony_ci	base->direction = 0;
238c2ecf20Sopenharmony_ci}
248c2ecf20Sopenharmony_ci
258c2ecf20Sopenharmony_cistatic void setup_ret_submit_pdu(struct usbip_header *rpdu, struct urbp *urb_p)
268c2ecf20Sopenharmony_ci{
278c2ecf20Sopenharmony_ci	setup_base_pdu(&rpdu->base, USBIP_RET_SUBMIT, urb_p->seqnum);
288c2ecf20Sopenharmony_ci	usbip_pack_pdu(rpdu, urb_p->urb, USBIP_RET_SUBMIT, 1);
298c2ecf20Sopenharmony_ci}
308c2ecf20Sopenharmony_ci
318c2ecf20Sopenharmony_cistatic void setup_ret_unlink_pdu(struct usbip_header *rpdu,
328c2ecf20Sopenharmony_ci				 struct v_unlink *unlink)
338c2ecf20Sopenharmony_ci{
348c2ecf20Sopenharmony_ci	setup_base_pdu(&rpdu->base, USBIP_RET_UNLINK, unlink->seqnum);
358c2ecf20Sopenharmony_ci	rpdu->u.ret_unlink.status = unlink->status;
368c2ecf20Sopenharmony_ci}
378c2ecf20Sopenharmony_ci
388c2ecf20Sopenharmony_cistatic int v_send_ret_unlink(struct vudc *udc, struct v_unlink *unlink)
398c2ecf20Sopenharmony_ci{
408c2ecf20Sopenharmony_ci	struct msghdr msg;
418c2ecf20Sopenharmony_ci	struct kvec iov[1];
428c2ecf20Sopenharmony_ci	size_t txsize;
438c2ecf20Sopenharmony_ci
448c2ecf20Sopenharmony_ci	int ret;
458c2ecf20Sopenharmony_ci	struct usbip_header pdu_header;
468c2ecf20Sopenharmony_ci
478c2ecf20Sopenharmony_ci	txsize = 0;
488c2ecf20Sopenharmony_ci	memset(&pdu_header, 0, sizeof(pdu_header));
498c2ecf20Sopenharmony_ci	memset(&msg, 0, sizeof(msg));
508c2ecf20Sopenharmony_ci	memset(&iov, 0, sizeof(iov));
518c2ecf20Sopenharmony_ci
528c2ecf20Sopenharmony_ci	/* 1. setup usbip_header */
538c2ecf20Sopenharmony_ci	setup_ret_unlink_pdu(&pdu_header, unlink);
548c2ecf20Sopenharmony_ci	usbip_header_correct_endian(&pdu_header, 1);
558c2ecf20Sopenharmony_ci
568c2ecf20Sopenharmony_ci	iov[0].iov_base = &pdu_header;
578c2ecf20Sopenharmony_ci	iov[0].iov_len  = sizeof(pdu_header);
588c2ecf20Sopenharmony_ci	txsize += sizeof(pdu_header);
598c2ecf20Sopenharmony_ci
608c2ecf20Sopenharmony_ci	ret = kernel_sendmsg(udc->ud.tcp_socket, &msg, iov,
618c2ecf20Sopenharmony_ci			     1, txsize);
628c2ecf20Sopenharmony_ci	if (ret != txsize) {
638c2ecf20Sopenharmony_ci		usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
648c2ecf20Sopenharmony_ci		if (ret >= 0)
658c2ecf20Sopenharmony_ci			return -EPIPE;
668c2ecf20Sopenharmony_ci		return ret;
678c2ecf20Sopenharmony_ci	}
688c2ecf20Sopenharmony_ci	kfree(unlink);
698c2ecf20Sopenharmony_ci
708c2ecf20Sopenharmony_ci	return txsize;
718c2ecf20Sopenharmony_ci}
728c2ecf20Sopenharmony_ci
738c2ecf20Sopenharmony_cistatic int v_send_ret_submit(struct vudc *udc, struct urbp *urb_p)
748c2ecf20Sopenharmony_ci{
758c2ecf20Sopenharmony_ci	struct urb *urb = urb_p->urb;
768c2ecf20Sopenharmony_ci	struct usbip_header pdu_header;
778c2ecf20Sopenharmony_ci	struct usbip_iso_packet_descriptor *iso_buffer = NULL;
788c2ecf20Sopenharmony_ci	struct kvec *iov = NULL;
798c2ecf20Sopenharmony_ci	int iovnum = 0;
808c2ecf20Sopenharmony_ci	int ret = 0;
818c2ecf20Sopenharmony_ci	size_t txsize;
828c2ecf20Sopenharmony_ci	struct msghdr msg;
838c2ecf20Sopenharmony_ci
848c2ecf20Sopenharmony_ci	txsize = 0;
858c2ecf20Sopenharmony_ci	memset(&pdu_header, 0, sizeof(pdu_header));
868c2ecf20Sopenharmony_ci	memset(&msg, 0, sizeof(msg));
878c2ecf20Sopenharmony_ci
888c2ecf20Sopenharmony_ci	if (urb->actual_length > 0 && !urb->transfer_buffer) {
898c2ecf20Sopenharmony_ci		dev_err(&udc->gadget.dev,
908c2ecf20Sopenharmony_ci			"urb: actual_length %d transfer_buffer null\n",
918c2ecf20Sopenharmony_ci			urb->actual_length);
928c2ecf20Sopenharmony_ci		return -1;
938c2ecf20Sopenharmony_ci	}
948c2ecf20Sopenharmony_ci
958c2ecf20Sopenharmony_ci	if (urb_p->type == USB_ENDPOINT_XFER_ISOC)
968c2ecf20Sopenharmony_ci		iovnum = 2 + urb->number_of_packets;
978c2ecf20Sopenharmony_ci	else
988c2ecf20Sopenharmony_ci		iovnum = 2;
998c2ecf20Sopenharmony_ci
1008c2ecf20Sopenharmony_ci	iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
1018c2ecf20Sopenharmony_ci	if (!iov) {
1028c2ecf20Sopenharmony_ci		usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_MALLOC);
1038c2ecf20Sopenharmony_ci		ret = -ENOMEM;
1048c2ecf20Sopenharmony_ci		goto out;
1058c2ecf20Sopenharmony_ci	}
1068c2ecf20Sopenharmony_ci	iovnum = 0;
1078c2ecf20Sopenharmony_ci
1088c2ecf20Sopenharmony_ci	/* 1. setup usbip_header */
1098c2ecf20Sopenharmony_ci	setup_ret_submit_pdu(&pdu_header, urb_p);
1108c2ecf20Sopenharmony_ci	usbip_dbg_stub_tx("setup txdata seqnum: %d\n",
1118c2ecf20Sopenharmony_ci			  pdu_header.base.seqnum);
1128c2ecf20Sopenharmony_ci	usbip_header_correct_endian(&pdu_header, 1);
1138c2ecf20Sopenharmony_ci
1148c2ecf20Sopenharmony_ci	iov[iovnum].iov_base = &pdu_header;
1158c2ecf20Sopenharmony_ci	iov[iovnum].iov_len  = sizeof(pdu_header);
1168c2ecf20Sopenharmony_ci	iovnum++;
1178c2ecf20Sopenharmony_ci	txsize += sizeof(pdu_header);
1188c2ecf20Sopenharmony_ci
1198c2ecf20Sopenharmony_ci	/* 2. setup transfer buffer */
1208c2ecf20Sopenharmony_ci	if (urb_p->type != USB_ENDPOINT_XFER_ISOC &&
1218c2ecf20Sopenharmony_ci	    usb_pipein(urb->pipe) && urb->actual_length > 0) {
1228c2ecf20Sopenharmony_ci		iov[iovnum].iov_base = urb->transfer_buffer;
1238c2ecf20Sopenharmony_ci		iov[iovnum].iov_len  = urb->actual_length;
1248c2ecf20Sopenharmony_ci		iovnum++;
1258c2ecf20Sopenharmony_ci		txsize += urb->actual_length;
1268c2ecf20Sopenharmony_ci	} else if (urb_p->type == USB_ENDPOINT_XFER_ISOC &&
1278c2ecf20Sopenharmony_ci		usb_pipein(urb->pipe)) {
1288c2ecf20Sopenharmony_ci		/* FIXME - copypasted from stub_tx, refactor */
1298c2ecf20Sopenharmony_ci		int i;
1308c2ecf20Sopenharmony_ci
1318c2ecf20Sopenharmony_ci		for (i = 0; i < urb->number_of_packets; i++) {
1328c2ecf20Sopenharmony_ci			iov[iovnum].iov_base = urb->transfer_buffer +
1338c2ecf20Sopenharmony_ci				urb->iso_frame_desc[i].offset;
1348c2ecf20Sopenharmony_ci			iov[iovnum].iov_len =
1358c2ecf20Sopenharmony_ci				urb->iso_frame_desc[i].actual_length;
1368c2ecf20Sopenharmony_ci			iovnum++;
1378c2ecf20Sopenharmony_ci			txsize += urb->iso_frame_desc[i].actual_length;
1388c2ecf20Sopenharmony_ci		}
1398c2ecf20Sopenharmony_ci
1408c2ecf20Sopenharmony_ci		if (txsize != sizeof(pdu_header) + urb->actual_length) {
1418c2ecf20Sopenharmony_ci			usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
1428c2ecf20Sopenharmony_ci			ret = -EPIPE;
1438c2ecf20Sopenharmony_ci			goto out;
1448c2ecf20Sopenharmony_ci		}
1458c2ecf20Sopenharmony_ci	}
1468c2ecf20Sopenharmony_ci	/* else - no buffer to send */
1478c2ecf20Sopenharmony_ci
1488c2ecf20Sopenharmony_ci	/* 3. setup iso_packet_descriptor */
1498c2ecf20Sopenharmony_ci	if (urb_p->type == USB_ENDPOINT_XFER_ISOC) {
1508c2ecf20Sopenharmony_ci		ssize_t len = 0;
1518c2ecf20Sopenharmony_ci
1528c2ecf20Sopenharmony_ci		iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
1538c2ecf20Sopenharmony_ci		if (!iso_buffer) {
1548c2ecf20Sopenharmony_ci			usbip_event_add(&udc->ud,
1558c2ecf20Sopenharmony_ci					VUDC_EVENT_ERROR_MALLOC);
1568c2ecf20Sopenharmony_ci			ret = -ENOMEM;
1578c2ecf20Sopenharmony_ci			goto out;
1588c2ecf20Sopenharmony_ci		}
1598c2ecf20Sopenharmony_ci
1608c2ecf20Sopenharmony_ci		iov[iovnum].iov_base = iso_buffer;
1618c2ecf20Sopenharmony_ci		iov[iovnum].iov_len  = len;
1628c2ecf20Sopenharmony_ci		txsize += len;
1638c2ecf20Sopenharmony_ci		iovnum++;
1648c2ecf20Sopenharmony_ci	}
1658c2ecf20Sopenharmony_ci
1668c2ecf20Sopenharmony_ci	ret = kernel_sendmsg(udc->ud.tcp_socket, &msg,
1678c2ecf20Sopenharmony_ci						iov,  iovnum, txsize);
1688c2ecf20Sopenharmony_ci	if (ret != txsize) {
1698c2ecf20Sopenharmony_ci		usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
1708c2ecf20Sopenharmony_ci		if (ret >= 0)
1718c2ecf20Sopenharmony_ci			ret = -EPIPE;
1728c2ecf20Sopenharmony_ci		goto out;
1738c2ecf20Sopenharmony_ci	}
1748c2ecf20Sopenharmony_ci
1758c2ecf20Sopenharmony_ciout:
1768c2ecf20Sopenharmony_ci	kfree(iov);
1778c2ecf20Sopenharmony_ci	kfree(iso_buffer);
1788c2ecf20Sopenharmony_ci	free_urbp_and_urb(urb_p);
1798c2ecf20Sopenharmony_ci	if (ret < 0)
1808c2ecf20Sopenharmony_ci		return ret;
1818c2ecf20Sopenharmony_ci	return txsize;
1828c2ecf20Sopenharmony_ci}
1838c2ecf20Sopenharmony_ci
1848c2ecf20Sopenharmony_cistatic int v_send_ret(struct vudc *udc)
1858c2ecf20Sopenharmony_ci{
1868c2ecf20Sopenharmony_ci	unsigned long flags;
1878c2ecf20Sopenharmony_ci	struct tx_item *txi;
1888c2ecf20Sopenharmony_ci	size_t total_size = 0;
1898c2ecf20Sopenharmony_ci	int ret = 0;
1908c2ecf20Sopenharmony_ci
1918c2ecf20Sopenharmony_ci	spin_lock_irqsave(&udc->lock_tx, flags);
1928c2ecf20Sopenharmony_ci	while (!list_empty(&udc->tx_queue)) {
1938c2ecf20Sopenharmony_ci		txi = list_first_entry(&udc->tx_queue, struct tx_item,
1948c2ecf20Sopenharmony_ci				       tx_entry);
1958c2ecf20Sopenharmony_ci		list_del(&txi->tx_entry);
1968c2ecf20Sopenharmony_ci		spin_unlock_irqrestore(&udc->lock_tx, flags);
1978c2ecf20Sopenharmony_ci
1988c2ecf20Sopenharmony_ci		switch (txi->type) {
1998c2ecf20Sopenharmony_ci		case TX_SUBMIT:
2008c2ecf20Sopenharmony_ci			ret = v_send_ret_submit(udc, txi->s);
2018c2ecf20Sopenharmony_ci			break;
2028c2ecf20Sopenharmony_ci		case TX_UNLINK:
2038c2ecf20Sopenharmony_ci			ret = v_send_ret_unlink(udc, txi->u);
2048c2ecf20Sopenharmony_ci			break;
2058c2ecf20Sopenharmony_ci		}
2068c2ecf20Sopenharmony_ci		kfree(txi);
2078c2ecf20Sopenharmony_ci
2088c2ecf20Sopenharmony_ci		if (ret < 0)
2098c2ecf20Sopenharmony_ci			return ret;
2108c2ecf20Sopenharmony_ci
2118c2ecf20Sopenharmony_ci		total_size += ret;
2128c2ecf20Sopenharmony_ci
2138c2ecf20Sopenharmony_ci		spin_lock_irqsave(&udc->lock_tx, flags);
2148c2ecf20Sopenharmony_ci	}
2158c2ecf20Sopenharmony_ci
2168c2ecf20Sopenharmony_ci	spin_unlock_irqrestore(&udc->lock_tx, flags);
2178c2ecf20Sopenharmony_ci	return total_size;
2188c2ecf20Sopenharmony_ci}
2198c2ecf20Sopenharmony_ci
2208c2ecf20Sopenharmony_ci
2218c2ecf20Sopenharmony_ciint v_tx_loop(void *data)
2228c2ecf20Sopenharmony_ci{
2238c2ecf20Sopenharmony_ci	struct usbip_device *ud = (struct usbip_device *) data;
2248c2ecf20Sopenharmony_ci	struct vudc *udc = container_of(ud, struct vudc, ud);
2258c2ecf20Sopenharmony_ci	int ret;
2268c2ecf20Sopenharmony_ci
2278c2ecf20Sopenharmony_ci	while (!kthread_should_stop()) {
2288c2ecf20Sopenharmony_ci		if (usbip_event_happened(&udc->ud))
2298c2ecf20Sopenharmony_ci			break;
2308c2ecf20Sopenharmony_ci		ret = v_send_ret(udc);
2318c2ecf20Sopenharmony_ci		if (ret < 0) {
2328c2ecf20Sopenharmony_ci			pr_warn("v_tx exit with error %d", ret);
2338c2ecf20Sopenharmony_ci			break;
2348c2ecf20Sopenharmony_ci		}
2358c2ecf20Sopenharmony_ci		wait_event_interruptible(udc->tx_waitq,
2368c2ecf20Sopenharmony_ci					 (!list_empty(&udc->tx_queue) ||
2378c2ecf20Sopenharmony_ci					 kthread_should_stop()));
2388c2ecf20Sopenharmony_ci	}
2398c2ecf20Sopenharmony_ci
2408c2ecf20Sopenharmony_ci	return 0;
2418c2ecf20Sopenharmony_ci}
2428c2ecf20Sopenharmony_ci
2438c2ecf20Sopenharmony_ci/* called with spinlocks held */
2448c2ecf20Sopenharmony_civoid v_enqueue_ret_unlink(struct vudc *udc, __u32 seqnum, __u32 status)
2458c2ecf20Sopenharmony_ci{
2468c2ecf20Sopenharmony_ci	struct tx_item *txi;
2478c2ecf20Sopenharmony_ci	struct v_unlink *unlink;
2488c2ecf20Sopenharmony_ci
2498c2ecf20Sopenharmony_ci	txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
2508c2ecf20Sopenharmony_ci	if (!txi) {
2518c2ecf20Sopenharmony_ci		usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
2528c2ecf20Sopenharmony_ci		return;
2538c2ecf20Sopenharmony_ci	}
2548c2ecf20Sopenharmony_ci	unlink = kzalloc(sizeof(*unlink), GFP_ATOMIC);
2558c2ecf20Sopenharmony_ci	if (!unlink) {
2568c2ecf20Sopenharmony_ci		kfree(txi);
2578c2ecf20Sopenharmony_ci		usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
2588c2ecf20Sopenharmony_ci		return;
2598c2ecf20Sopenharmony_ci	}
2608c2ecf20Sopenharmony_ci
2618c2ecf20Sopenharmony_ci	unlink->seqnum = seqnum;
2628c2ecf20Sopenharmony_ci	unlink->status = status;
2638c2ecf20Sopenharmony_ci	txi->type = TX_UNLINK;
2648c2ecf20Sopenharmony_ci	txi->u = unlink;
2658c2ecf20Sopenharmony_ci
2668c2ecf20Sopenharmony_ci	list_add_tail(&txi->tx_entry, &udc->tx_queue);
2678c2ecf20Sopenharmony_ci}
2688c2ecf20Sopenharmony_ci
2698c2ecf20Sopenharmony_ci/* called with spinlocks held */
2708c2ecf20Sopenharmony_civoid v_enqueue_ret_submit(struct vudc *udc, struct urbp *urb_p)
2718c2ecf20Sopenharmony_ci{
2728c2ecf20Sopenharmony_ci	struct tx_item *txi;
2738c2ecf20Sopenharmony_ci
2748c2ecf20Sopenharmony_ci	txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
2758c2ecf20Sopenharmony_ci	if (!txi) {
2768c2ecf20Sopenharmony_ci		usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
2778c2ecf20Sopenharmony_ci		return;
2788c2ecf20Sopenharmony_ci	}
2798c2ecf20Sopenharmony_ci
2808c2ecf20Sopenharmony_ci	txi->type = TX_SUBMIT;
2818c2ecf20Sopenharmony_ci	txi->s = urb_p;
2828c2ecf20Sopenharmony_ci
2838c2ecf20Sopenharmony_ci	list_add_tail(&txi->tx_entry, &udc->tx_queue);
2848c2ecf20Sopenharmony_ci}
285