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 kfree(iso_buffer); 151 usbip_dbg_vhci_tx("send txdata\n"); 152 153 total_size += txsize; 154 } 155 156 return total_size; 157 158 err_tx: 159 kfree(iso_buffer); 160 err_iso_buffer: 161 kfree(iov); 162 163 return err; 164 } 165 166 static struct vhci_unlink *dequeue_from_unlink_tx(struct vhci_device *vdev) 167 { 168 struct vhci_unlink *unlink, *tmp; 169 unsigned long flags; 170 171 spin_lock_irqsave(&vdev->priv_lock, flags); 172 173 list_for_each_entry_safe(unlink, tmp, &vdev->unlink_tx, list) { 174 list_move_tail(&unlink->list, &vdev->unlink_rx); 175 spin_unlock_irqrestore(&vdev->priv_lock, flags); 176 return unlink; 177 } 178 179 spin_unlock_irqrestore(&vdev->priv_lock, flags); 180 181 return NULL; 182 } 183 184 static int vhci_send_cmd_unlink(struct vhci_device *vdev) 185 { 186 struct vhci_unlink *unlink = NULL; 187 188 struct msghdr msg; 189 struct kvec iov; 190 size_t txsize; 191 size_t total_size = 0; 192 193 while ((unlink = dequeue_from_unlink_tx(vdev)) != NULL) { 194 int ret; 195 struct usbip_header pdu_header; 196 197 memset(&pdu_header, 0, sizeof(pdu_header)); 198 memset(&msg, 0, sizeof(msg)); 199 memset(&iov, 0, sizeof(iov)); 200 201 usbip_dbg_vhci_tx("setup cmd unlink, %lu\n", unlink->seqnum); 202 203 /* 1. setup usbip_header */ 204 pdu_header.base.command = USBIP_CMD_UNLINK; 205 pdu_header.base.seqnum = unlink->seqnum; 206 pdu_header.base.devid = vdev->devid; 207 pdu_header.base.ep = 0; 208 pdu_header.u.cmd_unlink.seqnum = unlink->unlink_seqnum; 209 210 usbip_header_correct_endian(&pdu_header, 1); 211 212 iov.iov_base = &pdu_header; 213 iov.iov_len = sizeof(pdu_header); 214 txsize = sizeof(pdu_header); 215 216 ret = kernel_sendmsg(vdev->ud.tcp_socket, &msg, &iov, 1, txsize); 217 if (ret != txsize) { 218 pr_err("sendmsg failed!, ret=%d for %zd\n", ret, 219 txsize); 220 usbip_event_add(&vdev->ud, VDEV_EVENT_ERROR_TCP); 221 return -1; 222 } 223 224 usbip_dbg_vhci_tx("send txdata\n"); 225 226 total_size += txsize; 227 } 228 229 return total_size; 230 } 231 232 int vhci_tx_loop(void *data) 233 { 234 struct usbip_device *ud = data; 235 struct vhci_device *vdev = container_of(ud, struct vhci_device, ud); 236 237 while (!kthread_should_stop()) { 238 if (vhci_send_cmd_submit(vdev) < 0) 239 break; 240 241 if (vhci_send_cmd_unlink(vdev) < 0) 242 break; 243 244 wait_event_interruptible(vdev->waitq_tx, 245 (!list_empty(&vdev->priv_tx) || 246 !list_empty(&vdev->unlink_tx) || 247 kthread_should_stop())); 248 249 usbip_dbg_vhci_tx("pending urbs ?, now wake up\n"); 250 } 251 252 return 0; 253 } 254