usb/usbip/vhci_tx.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * Copyright (C) 2003-2008 Takahiro Hirofuchi
0004  */
0005
0006 #include <linux/kthread.h>
0007 #include <linux/slab.h>
0008 #include <linux/scatterlist.h>
0009
0010 #include "usbip_common.h"
0011 #include "vhci.h"
0012
0013 static void setup_cmd_submit_pdu(struct usbip_header *pdup,  struct urb *urb)
0014 {
0015     struct vhci_priv *priv = ((struct vhci_priv *)urb->hcpriv);
0016     struct vhci_device *vdev = priv->vdev;
0017
0018     usbip_dbg_vhci_tx("URB, local devnum %u, remote devid %u\n",
0019               usb_pipedevice(urb->pipe), vdev->devid);
0020
0021     pdup->base.command   = USBIP_CMD_SUBMIT;
0022     pdup->base.seqnum    = priv->seqnum;
0023     pdup->base.devid     = vdev->devid;
0024     pdup->base.direction = usb_pipein(urb->pipe) ?
0025         USBIP_DIR_IN : USBIP_DIR_OUT;
0026     pdup->base.ep        = usb_pipeendpoint(urb->pipe);
0027
0028     usbip_pack_pdu(pdup, urb, USBIP_CMD_SUBMIT, 1);
0029
0030     if (urb->setup_packet)
0031         memcpy(pdup->u.cmd_submit.setup, urb->setup_packet, 8);
0032 }
0033
0034 static struct vhci_priv *dequeue_from_priv_tx(struct vhci_device *vdev)
0035 {
0036     struct vhci_priv *priv, *tmp;
0037     unsigned long flags;
0038
0039     spin_lock_irqsave(&vdev->priv_lock, flags);
0040
0041     list_for_each_entry_safe(priv, tmp, &vdev->priv_tx, list) {
0042         list_move_tail(&priv->list, &vdev->priv_rx);
0043         spin_unlock_irqrestore(&vdev->priv_lock, flags);
0044         return priv;
0045     }
0046
0047     spin_unlock_irqrestore(&vdev->priv_lock, flags);
0048
0049     return NULL;
0050 }
0051
0052 static int vhci_send_cmd_submit(struct vhci_device *vdev)
0053 {
0054     struct usbip_iso_packet_descriptor *iso_buffer = NULL;
0055     struct vhci_priv *priv = NULL;
0056     struct scatterlist *sg;
0057
0058     struct msghdr msg;
0059     struct kvec *iov;
0060     size_t txsize;
0061
0062     size_t total_size = 0;
0063     int iovnum;
0064     int err = -ENOMEM;
0065     int i;
0066
0067     while ((priv = dequeue_from_priv_tx(vdev)) != NULL) {
0068         int ret;
0069         struct urb *urb = priv->urb;
0070         struct usbip_header pdu_header;
0071
0072         txsize = 0;
0073         memset(&pdu_header, 0, sizeof(pdu_header));
0074         memset(&msg, 0, sizeof(msg));
0075         memset(&iov, 0, sizeof(iov));
0076
0077         usbip_dbg_vhci_tx("setup txdata urb seqnum %lu\n",
0078                   priv->seqnum);
0079
0080         if (urb->num_sgs && usb_pipeout(urb->pipe))
0081             iovnum = 2 + urb->num_sgs;
0082         else
0083             iovnum = 3;
0084
0085         iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
0086         if (!iov) {
0087             usbip_event_add(&vdev->ud, SDEV_EVENT_ERROR_MALLOC);
0088             return -ENOMEM;
0089         }
0090
0091         if (urb->num_sgs)
0092             urb->transfer_flags |= URB_DMA_MAP_SG;
0093
0094         /* 1. setup usbip_header */
0095         setup_cmd_submit_pdu(&pdu_header, urb);
0096         usbip_header_correct_endian(&pdu_header, 1);
0097         iovnum = 0;
0098
0099         iov[iovnum].iov_base = &pdu_header;
0100         iov[iovnum].iov_len  = sizeof(pdu_header);
0101         txsize += sizeof(pdu_header);
0102         iovnum++;
0103
0104         /* 2. setup transfer buffer */
0105         if (!usb_pipein(urb->pipe) && urb->transfer_buffer_length > 0) {
0106             if (urb->num_sgs &&
0107                       !usb_endpoint_xfer_isoc(&urb->ep->desc)) {
0108                 for_each_sg(urb->sg, sg, urb->num_sgs, i) {
0109                     iov[iovnum].iov_base = sg_virt(sg);
0110                     iov[iovnum].iov_len = sg->length;
0111                     iovnum++;
0112                 }
0113             } else {
0114                 iov[iovnum].iov_base = urb->transfer_buffer;
0115                 iov[iovnum].iov_len  =
0116                         urb->transfer_buffer_length;
0117                 iovnum++;
0118             }
0119             txsize += urb->transfer_buffer_length;
0120         }
0121
0122         /* 3. setup iso_packet_descriptor */
0123         if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
0124             ssize_t len = 0;
0125
0126             iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
0127             if (!iso_buffer) {
0128                 usbip_event_add(&vdev->ud,
0129                         SDEV_EVENT_ERROR_MALLOC);
0130                 goto err_iso_buffer;
0131             }
0132
0133             iov[iovnum].iov_base = iso_buffer;
0134             iov[iovnum].iov_len  = len;
0135             iovnum++;
0136             txsize += len;
0137         }
0138
0139         ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, iov, iovnum,
0140                      txsize);
0141         if (ret != txsize) {
0142             pr_err("sendmsg failed!, ret=%d for %zd\n", ret,
0143                    txsize);
0144             usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP);
0145             err = -EPIPE;
0146             goto err_tx;
0147         }
0148
0149         kfree(iov);
0150         /* This is only for isochronous case */
0151         kfree(iso_buffer);
0152         iso_buffer = NULL;
0153
0154         usbip_dbg_vhci_tx("send txdata\n");
0155
0156         total_size += txsize;
0157     }
0158
0159     return total_size;
0160
0161 err_tx:
0162     kfree(iso_buffer);
0163 err_iso_buffer:
0164     kfree(iov);
0165
0166     return err;
0167 }
0168
0169 static struct vhci_unlink *dequeue_from_unlink_tx(struct vhci_device *vdev)
0170 {
0171     struct vhci_unlink *unlink, *tmp;
0172     unsigned long flags;
0173
0174     spin_lock_irqsave(&vdev->priv_lock, flags);
0175
0176     list_for_each_entry_safe(unlink, tmp, &vdev->unlink_tx, list) {
0177         list_move_tail(&unlink->list, &vdev->unlink_rx);
0178         spin_unlock_irqrestore(&vdev->priv_lock, flags);
0179         return unlink;
0180     }
0181
0182     spin_unlock_irqrestore(&vdev->priv_lock, flags);
0183
0184     return NULL;
0185 }
0186
0187 static int vhci_send_cmd_unlink(struct vhci_device *vdev)
0188 {
0189     struct vhci_unlink *unlink = NULL;
0190
0191     struct msghdr msg;
0192     struct kvec iov;
0193     size_t txsize;
0194     size_t total_size = 0;
0195
0196     while ((unlink = dequeue_from_unlink_tx(vdev)) != NULL) {
0197         int ret;
0198         struct usbip_header pdu_header;
0199
0200         memset(&pdu_header, 0, sizeof(pdu_header));
0201         memset(&msg, 0, sizeof(msg));
0202         memset(&iov, 0, sizeof(iov));
0203
0204         usbip_dbg_vhci_tx("setup cmd unlink, %lu\n", unlink->seqnum);
0205
0206         /* 1. setup usbip_header */
0207         pdu_header.base.command = USBIP_CMD_UNLINK;
0208         pdu_header.base.seqnum  = unlink->seqnum;
0209         pdu_header.base.devid   = vdev->devid;
0210         pdu_header.base.ep  = 0;
0211         pdu_header.u.cmd_unlink.seqnum = unlink->unlink_seqnum;
0212
0213         usbip_header_correct_endian(&pdu_header, 1);
0214
0215         iov.iov_base = &pdu_header;
0216         iov.iov_len  = sizeof(pdu_header);
0217         txsize = sizeof(pdu_header);
0218
0219         ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, &iov, 1, txsize);
0220         if (ret != txsize) {
0221             pr_err("sendmsg failed!, ret=%d for %zd\n", ret,
0222                    txsize);
0223             usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP);
0224             return -1;
0225         }
0226
0227         usbip_dbg_vhci_tx("send txdata\n");
0228
0229         total_size += txsize;
0230     }
0231
0232     return total_size;
0233 }
0234
0235 int vhci_tx_loop(void *data)
0236 {
0237     struct usbip_device *ud = data;
0238     struct vhci_device *vdev = container_of(ud, struct vhci_device, ud);
0239
0240     while (!kthread_should_stop()) {
0241         if (vhci_send_cmd_submit(vdev) < 0)
0242             break;
0243
0244         if (vhci_send_cmd_unlink(vdev) < 0)
0245             break;
0246
0247         wait_event_interruptible(vdev->waitq_tx,
0248                      (!list_empty(&vdev->priv_tx) ||
0249                       !list_empty(&vdev->unlink_tx) ||
0250                       kthread_should_stop()));
0251
0252         usbip_dbg_vhci_tx("pending urbs ?, now wake up\n");
0253     }
0254
0255     return 0;
0256 }