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 } 257