drivers/usb/usbip/vhci_tx.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2003-2008 Takahiro Hirofuchi
   4  */
   5
   6 #include <linux/kthread.h>
   7 #include <linux/slab.h>
   8 #include <linux/scatterlist.h>
   9
  10 #include "usbip_common.h"
  11 #include "vhci.h"
  12
  13 static void setup_cmd_submit_pdu(struct usbip_header *pdup,  struct urb *urb)
  14 {
  15         struct vhci_priv *priv = ((struct vhci_priv *)urb->hcpriv);
  16         struct vhci_device *vdev = priv->vdev;
  17
  18         usbip_dbg_vhci_tx("URB, local devnum %u, remote devid %u\n",
  19                           usb_pipedevice(urb->pipe), vdev->devid);
  20
  21         pdup->base.command   = USBIP_CMD_SUBMIT;
  22         pdup->base.seqnum    = priv->seqnum;
  23         pdup->base.devid     = vdev->devid;
  24         pdup->base.direction = usb_pipein(urb->pipe) ?
  25                 USBIP_DIR_IN : USBIP_DIR_OUT;
  26         pdup->base.ep        = usb_pipeendpoint(urb->pipe);
  27
  28         usbip_pack_pdu(pdup, urb, USBIP_CMD_SUBMIT, 1);
  29
  30         if (urb->setup_packet)
  31                 memcpy(pdup->u.cmd_submit.setup, urb->setup_packet, 8);
  32 }
  33
  34 static struct vhci_priv *dequeue_from_priv_tx(struct vhci_device *vdev)
  35 {
  36         struct vhci_priv *priv, *tmp;
  37         unsigned long flags;
  38
  39         spin_lock_irqsave(&vdev->priv_lock, flags);
  40
  41         list_for_each_entry_safe(priv, tmp, &vdev->priv_tx, list) {
  42                 list_move_tail(&priv->list, &vdev->priv_rx);
  43                 spin_unlock_irqrestore(&vdev->priv_lock, flags);
  44                 return priv;
  45         }
  46
  47         spin_unlock_irqrestore(&vdev->priv_lock, flags);
  48
  49         return NULL;
  50 }
  51
  52 static int vhci_send_cmd_submit(struct vhci_device *vdev)
  53 {
  54         struct usbip_iso_packet_descriptor *iso_buffer = NULL;
  55         struct vhci_priv *priv = NULL;
  56         struct scatterlist *sg;
  57
  58         struct msghdr msg;
  59         struct kvec *iov;
  60         size_t txsize;
  61
  62         size_t total_size = 0;
  63         int iovnum;
  64         int err = -ENOMEM;
  65         int i;
  66
  67         while ((priv = dequeue_from_priv_tx(vdev)) != NULL) {
  68                 int ret;
  69                 struct urb *urb = priv->urb;
  70                 struct usbip_header pdu_header;
  71
  72                 txsize = 0;
  73                 memset(&pdu_header, 0, sizeof(pdu_header));
  74                 memset(&msg, 0, sizeof(msg));
  75                 memset(&iov, 0, sizeof(iov));
  76
  77                 usbip_dbg_vhci_tx("setup txdata urb seqnum %lu\n",
  78                                   priv->seqnum);
  79
  80                 if (urb->num_sgs && usb_pipeout(urb->pipe))
  81                         iovnum = 2 + urb->num_sgs;
  82                 else
  83                         iovnum = 3;
  84
  85                 iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
  86                 if (!iov) {
  87                         usbip_event_add(&vdev->ud, SDEV_EVENT_ERROR_MALLOC);
  88                         return -ENOMEM;
  89                 }
  90
  91                 if (urb->num_sgs)
  92                         urb->transfer_flags |= URB_DMA_MAP_SG;
  93
  94                 /* 1. setup usbip_header */
  95                 setup_cmd_submit_pdu(&pdu_header, urb);
  96                 usbip_header_correct_endian(&pdu_header, 1);
  97                 iovnum = 0;
  98
  99                 iov[iovnum].iov_base = &pdu_header;
 100                 iov[iovnum].iov_len  = sizeof(pdu_header);
 101                 txsize += sizeof(pdu_header);
 102                 iovnum++;
 103
 104                 /* 2. setup transfer buffer */
 105                 if (!usb_pipein(urb->pipe) && urb->transfer_buffer_length > 0) {
 106                         if (urb->num_sgs &&
 107                                       !usb_endpoint_xfer_isoc(&urb->ep->desc)) {
 108                                 for_each_sg(urb->sg, sg, urb->num_sgs, i) {
 109                                         iov[iovnum].iov_base = sg_virt(sg);
 110                                         iov[iovnum].iov_len = sg->length;
 111                                         iovnum++;
 112                                 }
 113                         } else {
 114                                 iov[iovnum].iov_base = urb->transfer_buffer;
 115                                 iov[iovnum].iov_len  =
 116                                                 urb->transfer_buffer_length;
 117                                 iovnum++;
 118                         }
 119                         txsize += urb->transfer_buffer_length;
 120                 }
 121
 122                 /* 3. setup iso_packet_descriptor */
 123                 if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
 124                         ssize_t len = 0;
 125
 126                         iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
 127                         if (!iso_buffer) {
 128                                 usbip_event_add(&vdev->ud,
 129                                                 SDEV_EVENT_ERROR_MALLOC);
 130                                 goto err_iso_buffer;
 131                         }
 132
 133                         iov[iovnum].iov_base = iso_buffer;
 134                         iov[iovnum].iov_len  = len;
 135                         iovnum++;
 136                         txsize += len;
 137                 }
 138
 139                 ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, iov, iovnum,
 140                                      txsize);
 141                 if (ret != txsize) {
 142                         pr_err("sendmsg failed!, ret=%d for %zd\n", ret,
 143                                txsize);
 144                         usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP);
 145                         err = -EPIPE;
 146                         goto err_tx;
 147                 }
 148
 149                 kfree(iov);
 150                 /* This is only for isochronous case */
 151                 kfree(iso_buffer);
 152                 iso_buffer = NULL;
 153
 154                 usbip_dbg_vhci_tx("send txdata\n");
 155
 156                 total_size += txsize;
 157         }
 158
 159         return total_size;
 160
 161 err_tx:
 162         kfree(iso_buffer);
 163 err_iso_buffer:
 164         kfree(iov);
 165
 166         return err;
 167 }
 168
 169 static struct vhci_unlink *dequeue_from_unlink_tx(struct vhci_device *vdev)
 170 {
 171         struct vhci_unlink *unlink, *tmp;
 172         unsigned long flags;
 173
 174         spin_lock_irqsave(&vdev->priv_lock, flags);
 175
 176         list_for_each_entry_safe(unlink, tmp, &vdev->unlink_tx, list) {
 177                 list_move_tail(&unlink->list, &vdev->unlink_rx);
 178                 spin_unlock_irqrestore(&vdev->priv_lock, flags);
 179                 return unlink;
 180         }
 181
 182         spin_unlock_irqrestore(&vdev->priv_lock, flags);
 183
 184         return NULL;
 185 }
 186
 187 static int vhci_send_cmd_unlink(struct vhci_device *vdev)
 188 {
 189         struct vhci_unlink *unlink = NULL;
 190
 191         struct msghdr msg;
 192         struct kvec iov;
 193         size_t txsize;
 194         size_t total_size = 0;
 195
 196         while ((unlink = dequeue_from_unlink_tx(vdev)) != NULL) {
 197                 int ret;
 198                 struct usbip_header pdu_header;
 199
 200                 memset(&pdu_header, 0, sizeof(pdu_header));
 201                 memset(&msg, 0, sizeof(msg));
 202                 memset(&iov, 0, sizeof(iov));
 203
 204                 usbip_dbg_vhci_tx("setup cmd unlink, %lu\n", unlink->seqnum);
 205
 206                 /* 1. setup usbip_header */
 207                 pdu_header.base.command = USBIP_CMD_UNLINK;
 208                 pdu_header.base.seqnum  = unlink->seqnum;
 209                 pdu_header.base.devid   = vdev->devid;
 210                 pdu_header.base.ep      = 0;
 211                 pdu_header.u.cmd_unlink.seqnum = unlink->unlink_seqnum;
 212
 213                 usbip_header_correct_endian(&pdu_header, 1);
 214
 215                 iov.iov_base = &pdu_header;
 216                 iov.iov_len  = sizeof(pdu_header);
 217                 txsize = sizeof(pdu_header);
 218
 219                 ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, &iov, 1, txsize);
 220                 if (ret != txsize) {
 221                         pr_err("sendmsg failed!, ret=%d for %zd\n", ret,
 222                                txsize);
 223                         usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP);
 224                         return -1;
 225                 }
 226
 227                 usbip_dbg_vhci_tx("send txdata\n");
 228
 229                 total_size += txsize;
 230         }
 231
 232         return total_size;
 233 }
 234
 235 int vhci_tx_loop(void *data)
 236 {
 237         struct usbip_device *ud = data;
 238         struct vhci_device *vdev = container_of(ud, struct vhci_device, ud);
 239
 240         while (!kthread_should_stop()) {
 241                 if (vhci_send_cmd_submit(vdev) < 0)
 242                         break;
 243
 244                 if (vhci_send_cmd_unlink(vdev) < 0)
 245                         break;
 246
 247                 wait_event_interruptible(vdev->waitq_tx,
 248                                          (!list_empty(&vdev->priv_tx) ||
 249                                           !list_empty(&vdev->unlink_tx) ||
 250                                           kthread_should_stop()));
 251
 252                 usbip_dbg_vhci_tx("pending urbs ?, now wake up\n");
 253         }
 254
 255         return 0;
 256 }