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