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