[mirror_ubuntu-bionic-kernel.git] / drivers / usb / usbip / vudc_tx.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright (C) 2015 Karol Kosik <karo9@interia.eu>
 * Copyright (C) 2015-2016 Samsung Electronics
 *               Igor Kotrasinski <i.kotrasinsk@samsung.com>
 */

#include <net/sock.h>
#include <linux/list.h>
#include <linux/kthread.h>

#include "usbip_common.h"
#include "vudc.h"

static inline void setup_base_pdu(struct usbip_header_basic *base,
				  __u32 command, __u32 seqnum)
{
	base->command	= command;
	base->seqnum	= seqnum;
	base->devid	= 0;
	base->ep	= 0;
	base->direction = 0;
}

static void setup_ret_submit_pdu(struct usbip_header *rpdu, struct urbp *urb_p)
{
	setup_base_pdu(&rpdu->base, USBIP_RET_SUBMIT, urb_p->seqnum);
	usbip_pack_pdu(rpdu, urb_p->urb, USBIP_RET_SUBMIT, 1);
}

static void setup_ret_unlink_pdu(struct usbip_header *rpdu,
				 struct v_unlink *unlink)
{
	setup_base_pdu(&rpdu->base, USBIP_RET_UNLINK, unlink->seqnum);
	rpdu->u.ret_unlink.status = unlink->status;
}

static int v_send_ret_unlink(struct vudc *udc, struct v_unlink *unlink)
{
	struct msghdr msg;
	struct kvec iov[1];
	size_t txsize;

	int ret;
	struct usbip_header pdu_header;

	txsize = 0;
	memset(&pdu_header, 0, sizeof(pdu_header));
	memset(&msg, 0, sizeof(msg));
	memset(&iov, 0, sizeof(iov));

	/* 1. setup usbip_header */
	setup_ret_unlink_pdu(&pdu_header, unlink);
	usbip_header_correct_endian(&pdu_header, 1);

	iov[0].iov_base = &pdu_header;
	iov[0].iov_len  = sizeof(pdu_header);
	txsize += sizeof(pdu_header);

	ret = kernel_sendmsg(udc->ud.tcp_socket, &msg, iov,
			     1, txsize);
	if (ret != txsize) {
		usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
		if (ret >= 0)
			return -EPIPE;
		return ret;
	}
	kfree(unlink);

	return txsize;
}

static int v_send_ret_submit(struct vudc *udc, struct urbp *urb_p)
{
	struct urb *urb = urb_p->urb;
	struct usbip_header pdu_header;
	struct usbip_iso_packet_descriptor *iso_buffer = NULL;
	struct kvec *iov = NULL;
	int iovnum = 0;
	int ret = 0;
	size_t txsize;
	struct msghdr msg;

	txsize = 0;
	memset(&pdu_header, 0, sizeof(pdu_header));
	memset(&msg, 0, sizeof(msg));

	if (urb->actual_length > 0 && !urb->transfer_buffer) {
		dev_err(&udc->gadget.dev,
			"urb: actual_length %d transfer_buffer null\n",
			urb->actual_length);
		return -1;
	}

	if (urb_p->type == USB_ENDPOINT_XFER_ISOC)
		iovnum = 2 + urb->number_of_packets;
	else
		iovnum = 2;

	iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
	if (!iov) {
		usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_MALLOC);
		ret = -ENOMEM;
		goto out;
	}
	iovnum = 0;

	/* 1. setup usbip_header */
	setup_ret_submit_pdu(&pdu_header, urb_p);
	usbip_dbg_stub_tx("setup txdata seqnum: %d\n",
			  pdu_header.base.seqnum);
	usbip_header_correct_endian(&pdu_header, 1);

	iov[iovnum].iov_base = &pdu_header;
	iov[iovnum].iov_len  = sizeof(pdu_header);
	iovnum++;
	txsize += sizeof(pdu_header);

	/* 2. setup transfer buffer */
	if (urb_p->type != USB_ENDPOINT_XFER_ISOC &&
	    usb_pipein(urb->pipe) && urb->actual_length > 0) {
		iov[iovnum].iov_base = urb->transfer_buffer;
		iov[iovnum].iov_len  = urb->actual_length;
		iovnum++;
		txsize += urb->actual_length;
	} else if (urb_p->type == USB_ENDPOINT_XFER_ISOC &&
		usb_pipein(urb->pipe)) {
		/* FIXME - copypasted from stub_tx, refactor */
		int i;

		for (i = 0; i < urb->number_of_packets; i++) {
			iov[iovnum].iov_base = urb->transfer_buffer +
				urb->iso_frame_desc[i].offset;
			iov[iovnum].iov_len =
				urb->iso_frame_desc[i].actual_length;
			iovnum++;
			txsize += urb->iso_frame_desc[i].actual_length;
		}

		if (txsize != sizeof(pdu_header) + urb->actual_length) {
			usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
			ret = -EPIPE;
			goto out;
		}
	}
	/* else - no buffer to send */

	/* 3. setup iso_packet_descriptor */
	if (urb_p->type == USB_ENDPOINT_XFER_ISOC) {
		ssize_t len = 0;

		iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
		if (!iso_buffer) {
			usbip_event_add(&udc->ud,
					VUDC_EVENT_ERROR_MALLOC);
			ret = -ENOMEM;
			goto out;
		}

		iov[iovnum].iov_base = iso_buffer;
		iov[iovnum].iov_len  = len;
		txsize += len;
		iovnum++;
	}

	ret = kernel_sendmsg(udc->ud.tcp_socket, &msg,
						iov,  iovnum, txsize);
	if (ret != txsize) {
		usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
		if (ret >= 0)
			ret = -EPIPE;
		goto out;
	}

out:
	kfree(iov);
	kfree(iso_buffer);
	free_urbp_and_urb(urb_p);
	if (ret < 0)
		return ret;
	return txsize;
}

static int v_send_ret(struct vudc *udc)
{
	unsigned long flags;
	struct tx_item *txi;
	size_t total_size = 0;
	int ret = 0;

	spin_lock_irqsave(&udc->lock_tx, flags);
	while (!list_empty(&udc->tx_queue)) {
		txi = list_first_entry(&udc->tx_queue, struct tx_item,
				       tx_entry);
		list_del(&txi->tx_entry);
		spin_unlock_irqrestore(&udc->lock_tx, flags);

		switch (txi->type) {
		case TX_SUBMIT:
			ret = v_send_ret_submit(udc, txi->s);
			break;
		case TX_UNLINK:
			ret = v_send_ret_unlink(udc, txi->u);
			break;
		}
		kfree(txi);

		if (ret < 0)
			return ret;

		total_size += ret;

		spin_lock_irqsave(&udc->lock_tx, flags);
	}

	spin_unlock_irqrestore(&udc->lock_tx, flags);
	return total_size;
}


int v_tx_loop(void *data)
{
	struct usbip_device *ud = (struct usbip_device *) data;
	struct vudc *udc = container_of(ud, struct vudc, ud);
	int ret;

	while (!kthread_should_stop()) {
		if (usbip_event_happened(&udc->ud))
			break;
		ret = v_send_ret(udc);
		if (ret < 0) {
			pr_warn("v_tx exit with error %d", ret);
			break;
		}
		wait_event_interruptible(udc->tx_waitq,
					 (!list_empty(&udc->tx_queue) ||
					 kthread_should_stop()));
	}

	return 0;
}

/* called with spinlocks held */
void v_enqueue_ret_unlink(struct vudc *udc, __u32 seqnum, __u32 status)
{
	struct tx_item *txi;
	struct v_unlink *unlink;

	txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
	if (!txi) {
		usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
		return;
	}
	unlink = kzalloc(sizeof(*unlink), GFP_ATOMIC);
	if (!unlink) {
		kfree(txi);
		usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
		return;
	}

	unlink->seqnum = seqnum;
	unlink->status = status;
	txi->type = TX_UNLINK;
	txi->u = unlink;

	list_add_tail(&txi->tx_entry, &udc->tx_queue);
}

/* called with spinlocks held */
void v_enqueue_ret_submit(struct vudc *udc, struct urbp *urb_p)
{
	struct tx_item *txi;

	txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
	if (!txi) {
		usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
		return;
	}

	txi->type = TX_SUBMIT;
	txi->s = urb_p;

	list_add_tail(&txi->tx_entry, &udc->tx_queue);
}
Commit	Line	Data
5fd54ace	1	// SPDX-License-Identifier: GPL-2.0+
d62ba981 IK	2	/*
	3	* Copyright (C) 2015 Karol Kosik <karo9@interia.eu>
	4	* Copyright (C) 2015-2016 Samsung Electronics
	5	* Igor Kotrasinski <i.kotrasinsk@samsung.com>
d62ba981 IK	6	*/
	7
	8	#include <net/sock.h>
	9	#include <linux/list.h>
	10	#include <linux/kthread.h>
	11
	12	#include "usbip_common.h"
	13	#include "vudc.h"
	14
	15	static inline void setup_base_pdu(struct usbip_header_basic *base,
	16	__u32 command, __u32 seqnum)
	17	{
	18	base->command = command;
	19	base->seqnum = seqnum;
	20	base->devid = 0;
	21	base->ep = 0;
	22	base->direction = 0;
	23	}
	24
	25	static void setup_ret_submit_pdu(struct usbip_header rpdu, struct urbp urb_p)
	26	{
	27	setup_base_pdu(&rpdu->base, USBIP_RET_SUBMIT, urb_p->seqnum);
	28	usbip_pack_pdu(rpdu, urb_p->urb, USBIP_RET_SUBMIT, 1);
	29	}
	30
	31	static void setup_ret_unlink_pdu(struct usbip_header *rpdu,
	32	struct v_unlink *unlink)
	33	{
	34	setup_base_pdu(&rpdu->base, USBIP_RET_UNLINK, unlink->seqnum);
	35	rpdu->u.ret_unlink.status = unlink->status;
	36	}
	37
	38	static int v_send_ret_unlink(struct vudc udc, struct v_unlink unlink)
	39	{
	40	struct msghdr msg;
	41	struct kvec iov[1];
	42	size_t txsize;
	43
	44	int ret;
	45	struct usbip_header pdu_header;
	46
	47	txsize = 0;
	48	memset(&pdu_header, 0, sizeof(pdu_header));
	49	memset(&msg, 0, sizeof(msg));
	50	memset(&iov, 0, sizeof(iov));
	51
	52	/* 1. setup usbip_header */
	53	setup_ret_unlink_pdu(&pdu_header, unlink);
	54	usbip_header_correct_endian(&pdu_header, 1);
	55
	56	iov[0].iov_base = &pdu_header;
	57	iov[0].iov_len = sizeof(pdu_header);
	58	txsize += sizeof(pdu_header);
	59
	60	ret = kernel_sendmsg(udc->ud.tcp_socket, &msg, iov,
	61	1, txsize);
	62	if (ret != txsize) {
	63	usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
	64	if (ret >= 0)
	65	return -EPIPE;
	66	return ret;
	67	}
	68	kfree(unlink);
	69
70	return txsize;
71	}
72
73	static int v_send_ret_submit(struct vudc udc, struct urbp urb_p)
74	{
75	struct urb *urb = urb_p->urb;
76	struct usbip_header pdu_header;
77	struct usbip_iso_packet_descriptor *iso_buffer = NULL;
78	struct kvec *iov = NULL;
79	int iovnum = 0;
80	int ret = 0;
81	size_t txsize;
82	struct msghdr msg;
83
84	txsize = 0;
85	memset(&pdu_header, 0, sizeof(pdu_header));
86	memset(&msg, 0, sizeof(msg));
87
5fd77a3a SK	88	if (urb->actual_length > 0 && !urb->transfer_buffer) {
	89	dev_err(&udc->gadget.dev,
	90	"urb: actual_length %d transfer_buffer null\n",
	91	urb->actual_length);
	92	return -1;
	93	}
	94
d62ba981 IK	95	if (urb_p->type == USB_ENDPOINT_XFER_ISOC)
	96	iovnum = 2 + urb->number_of_packets;
	97	else
	98	iovnum = 2;
	99
	100	iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
	101	if (!iov) {
	102	usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_MALLOC);
	103	ret = -ENOMEM;
	104	goto out;
	105	}
	106	iovnum = 0;
	107
	108	/* 1. setup usbip_header */
	109	setup_ret_submit_pdu(&pdu_header, urb_p);
5fd77a3a SK	110	usbip_dbg_stub_tx("setup txdata seqnum: %d\n",
5fd77a3a SK	111	pdu_header.base.seqnum);
d62ba981 IK	112	usbip_header_correct_endian(&pdu_header, 1);
	113
	114	iov[iovnum].iov_base = &pdu_header;
	115	iov[iovnum].iov_len = sizeof(pdu_header);
	116	iovnum++;
	117	txsize += sizeof(pdu_header);
	118
	119	/* 2. setup transfer buffer */
	120	if (urb_p->type != USB_ENDPOINT_XFER_ISOC &&
	121	usb_pipein(urb->pipe) && urb->actual_length > 0) {
	122	iov[iovnum].iov_base = urb->transfer_buffer;
	123	iov[iovnum].iov_len = urb->actual_length;
	124	iovnum++;
	125	txsize += urb->actual_length;
	126	} else if (urb_p->type == USB_ENDPOINT_XFER_ISOC &&
	127	usb_pipein(urb->pipe)) {
	128	/* FIXME - copypasted from stub_tx, refactor */
	129	int i;
	130
	131	for (i = 0; i < urb->number_of_packets; i++) {
	132	iov[iovnum].iov_base = urb->transfer_buffer +
	133	urb->iso_frame_desc[i].offset;
	134	iov[iovnum].iov_len =
	135	urb->iso_frame_desc[i].actual_length;
	136	iovnum++;
	137	txsize += urb->iso_frame_desc[i].actual_length;
	138	}
	139
	140	if (txsize != sizeof(pdu_header) + urb->actual_length) {
	141	usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
	142	ret = -EPIPE;
	143	goto out;
	144	}
	145	}
	146	/* else - no buffer to send */
	147
	148	/* 3. setup iso_packet_descriptor */
	149	if (urb_p->type == USB_ENDPOINT_XFER_ISOC) {
	150	ssize_t len = 0;
	151
	152	iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
	153	if (!iso_buffer) {
	154	usbip_event_add(&udc->ud,
	155	VUDC_EVENT_ERROR_MALLOC);
	156	ret = -ENOMEM;
	157	goto out;
	158	}
	159
	160	iov[iovnum].iov_base = iso_buffer;
	161	iov[iovnum].iov_len = len;
	162	txsize += len;
	163	iovnum++;
	164	}
	165
	166	ret = kernel_sendmsg(udc->ud.tcp_socket, &msg,
	167	iov, iovnum, txsize);
	168	if (ret != txsize) {
	169	usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
	170	if (ret >= 0)
	171	ret = -EPIPE;
	172	goto out;
	173	}
	174
	175	out:
176	kfree(iov);
177	kfree(iso_buffer);
178	free_urbp_and_urb(urb_p);
179	if (ret < 0)
180	return ret;
181	return txsize;
182	}
183
184	static int v_send_ret(struct vudc *udc)
185	{
186	unsigned long flags;
187	struct tx_item *txi;
188	size_t total_size = 0;
189	int ret = 0;
190
191	spin_lock_irqsave(&udc->lock_tx, flags);
192	while (!list_empty(&udc->tx_queue)) {
193	txi = list_first_entry(&udc->tx_queue, struct tx_item,
194	tx_entry);
195	list_del(&txi->tx_entry);
196	spin_unlock_irqrestore(&udc->lock_tx, flags);
197
198	switch (txi->type) {
199	case TX_SUBMIT:
200	ret = v_send_ret_submit(udc, txi->s);
201	break;
202	case TX_UNLINK:
203	ret = v_send_ret_unlink(udc, txi->u);
204	break;
205	}
206	kfree(txi);
207
208	if (ret < 0)
209	return ret;
210
211	total_size += ret;
212
213	spin_lock_irqsave(&udc->lock_tx, flags);
214	}
215
216	spin_unlock_irqrestore(&udc->lock_tx, flags);
217	return total_size;
218	}
219
220
221	int v_tx_loop(void *data)
222	{
223	struct usbip_device ud = (struct usbip_device ) data;
224	struct vudc *udc = container_of(ud, struct vudc, ud);
225	int ret;
226
227	while (!kthread_should_stop()) {
228	if (usbip_event_happened(&udc->ud))
229	break;
230	ret = v_send_ret(udc);
231	if (ret < 0) {
232	pr_warn("v_tx exit with error %d", ret);
233	break;
234	}
235	wait_event_interruptible(udc->tx_waitq,
236	(!list_empty(&udc->tx_queue) \|\|
237	kthread_should_stop()));
238	}
239
240	return 0;
241	}
242
243	/* called with spinlocks held */
244	void v_enqueue_ret_unlink(struct vudc *udc, __u32 seqnum, __u32 status)
245	{
246	struct tx_item *txi;
247	struct v_unlink *unlink;
248
249	txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
250	if (!txi) {
251	usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
252	return;
253	}
254	unlink = kzalloc(sizeof(*unlink), GFP_ATOMIC);
255	if (!unlink) {
256	kfree(txi);
257	usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
258	return;
259	}
260
261	unlink->seqnum = seqnum;
262	unlink->status = status;
263	txi->type = TX_UNLINK;
264	txi->u = unlink;
265
266	list_add_tail(&txi->tx_entry, &udc->tx_queue);
267	}
268
269	/* called with spinlocks held */
270	void v_enqueue_ret_submit(struct vudc udc, struct urbp urb_p)
271	{
272	struct tx_item *txi;
273
274	txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
275	if (!txi) {
276	usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
277	return;
278	}
279
280	txi->type = TX_SUBMIT;
281	txi->s = urb_p;
282
283	list_add_tail(&txi->tx_entry, &udc->tx_queue);
284	}