usb/usbip/vudc_tx.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * Copyright (C) 2015 Karol Kosik <karo9@interia.eu>
0004  * Copyright (C) 2015-2016 Samsung Electronics
0005  *               Igor Kotrasinski <i.kotrasinsk@samsung.com>
0006  */
0007
0008 #include <net/sock.h>
0009 #include <linux/list.h>
0010 #include <linux/kthread.h>
0011
0012 #include "usbip_common.h"
0013 #include "vudc.h"
0014
0015 static inline void setup_base_pdu(struct usbip_header_basic *base,
0016                   __u32 command, __u32 seqnum)
0017 {
0018     base->command   = command;
0019     base->seqnum    = seqnum;
0020     base->devid = 0;
0021     base->ep    = 0;
0022     base->direction = 0;
0023 }
0024
0025 static void setup_ret_submit_pdu(struct usbip_header *rpdu, struct urbp *urb_p)
0026 {
0027     setup_base_pdu(&rpdu->base, USBIP_RET_SUBMIT, urb_p->seqnum);
0028     usbip_pack_pdu(rpdu, urb_p->urb, USBIP_RET_SUBMIT, 1);
0029 }
0030
0031 static void setup_ret_unlink_pdu(struct usbip_header *rpdu,
0032                  struct v_unlink *unlink)
0033 {
0034     setup_base_pdu(&rpdu->base, USBIP_RET_UNLINK, unlink->seqnum);
0035     rpdu->u.ret_unlink.status = unlink->status;
0036 }
0037
0038 static int v_send_ret_unlink(struct vudc *udc, struct v_unlink *unlink)
0039 {
0040     struct msghdr msg;
0041     struct kvec iov[1];
0042     size_t txsize;
0043
0044     int ret;
0045     struct usbip_header pdu_header;
0046
0047     txsize = 0;
0048     memset(&pdu_header, 0, sizeof(pdu_header));
0049     memset(&msg, 0, sizeof(msg));
0050     memset(&iov, 0, sizeof(iov));
0051
0052     /* 1. setup usbip_header */
0053     setup_ret_unlink_pdu(&pdu_header, unlink);
0054     usbip_header_correct_endian(&pdu_header, 1);
0055
0056     iov[0].iov_base = &pdu_header;
0057     iov[0].iov_len  = sizeof(pdu_header);
0058     txsize += sizeof(pdu_header);
0059
0060     ret = kernel_sendmsg(udc->ud.tcp_socket, &msg, iov,
0061                  1, txsize);
0062     if (ret != txsize) {
0063         usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
0064         if (ret >= 0)
0065             return -EPIPE;
0066         return ret;
0067     }
0068     kfree(unlink);
0069
0070     return txsize;
0071 }
0072
0073 static int v_send_ret_submit(struct vudc *udc, struct urbp *urb_p)
0074 {
0075     struct urb *urb = urb_p->urb;
0076     struct usbip_header pdu_header;
0077     struct usbip_iso_packet_descriptor *iso_buffer = NULL;
0078     struct kvec *iov = NULL;
0079     int iovnum = 0;
0080     int ret = 0;
0081     size_t txsize;
0082     struct msghdr msg;
0083
0084     txsize = 0;
0085     memset(&pdu_header, 0, sizeof(pdu_header));
0086     memset(&msg, 0, sizeof(msg));
0087
0088     if (urb->actual_length > 0 && !urb->transfer_buffer) {
0089         dev_err(&udc->gadget.dev,
0090             "urb: actual_length %d transfer_buffer null\n",
0091             urb->actual_length);
0092         return -1;
0093     }
0094
0095     if (urb_p->type == USB_ENDPOINT_XFER_ISOC)
0096         iovnum = 2 + urb->number_of_packets;
0097     else
0098         iovnum = 2;
0099
0100     iov = kcalloc(iovnum, sizeof(*iov), GFP_KERNEL);
0101     if (!iov) {
0102         usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_MALLOC);
0103         ret = -ENOMEM;
0104         goto out;
0105     }
0106     iovnum = 0;
0107
0108     /* 1. setup usbip_header */
0109     setup_ret_submit_pdu(&pdu_header, urb_p);
0110     usbip_dbg_stub_tx("setup txdata seqnum: %d\n",
0111               pdu_header.base.seqnum);
0112     usbip_header_correct_endian(&pdu_header, 1);
0113
0114     iov[iovnum].iov_base = &pdu_header;
0115     iov[iovnum].iov_len  = sizeof(pdu_header);
0116     iovnum++;
0117     txsize += sizeof(pdu_header);
0118
0119     /* 2. setup transfer buffer */
0120     if (urb_p->type != USB_ENDPOINT_XFER_ISOC &&
0121         usb_pipein(urb->pipe) && urb->actual_length > 0) {
0122         iov[iovnum].iov_base = urb->transfer_buffer;
0123         iov[iovnum].iov_len  = urb->actual_length;
0124         iovnum++;
0125         txsize += urb->actual_length;
0126     } else if (urb_p->type == USB_ENDPOINT_XFER_ISOC &&
0127         usb_pipein(urb->pipe)) {
0128         /* FIXME - copypasted from stub_tx, refactor */
0129         int i;
0130
0131         for (i = 0; i < urb->number_of_packets; i++) {
0132             iov[iovnum].iov_base = urb->transfer_buffer +
0133                 urb->iso_frame_desc[i].offset;
0134             iov[iovnum].iov_len =
0135                 urb->iso_frame_desc[i].actual_length;
0136             iovnum++;
0137             txsize += urb->iso_frame_desc[i].actual_length;
0138         }
0139
0140         if (txsize != sizeof(pdu_header) + urb->actual_length) {
0141             usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
0142             ret = -EPIPE;
0143             goto out;
0144         }
0145     }
0146     /* else - no buffer to send */
0147
0148     /* 3. setup iso_packet_descriptor */
0149     if (urb_p->type == USB_ENDPOINT_XFER_ISOC) {
0150         ssize_t len = 0;
0151
0152         iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
0153         if (!iso_buffer) {
0154             usbip_event_add(&udc->ud,
0155                     VUDC_EVENT_ERROR_MALLOC);
0156             ret = -ENOMEM;
0157             goto out;
0158         }
0159
0160         iov[iovnum].iov_base = iso_buffer;
0161         iov[iovnum].iov_len  = len;
0162         txsize += len;
0163         iovnum++;
0164     }
0165
0166     ret = kernel_sendmsg(udc->ud.tcp_socket, &msg,
0167                         iov,  iovnum, txsize);
0168     if (ret != txsize) {
0169         usbip_event_add(&udc->ud, VUDC_EVENT_ERROR_TCP);
0170         if (ret >= 0)
0171             ret = -EPIPE;
0172         goto out;
0173     }
0174
0175 out:
0176     kfree(iov);
0177     kfree(iso_buffer);
0178     free_urbp_and_urb(urb_p);
0179     if (ret < 0)
0180         return ret;
0181     return txsize;
0182 }
0183
0184 static int v_send_ret(struct vudc *udc)
0185 {
0186     unsigned long flags;
0187     struct tx_item *txi;
0188     size_t total_size = 0;
0189     int ret = 0;
0190
0191     spin_lock_irqsave(&udc->lock_tx, flags);
0192     while (!list_empty(&udc->tx_queue)) {
0193         txi = list_first_entry(&udc->tx_queue, struct tx_item,
0194                        tx_entry);
0195         list_del(&txi->tx_entry);
0196         spin_unlock_irqrestore(&udc->lock_tx, flags);
0197
0198         switch (txi->type) {
0199         case TX_SUBMIT:
0200             ret = v_send_ret_submit(udc, txi->s);
0201             break;
0202         case TX_UNLINK:
0203             ret = v_send_ret_unlink(udc, txi->u);
0204             break;
0205         }
0206         kfree(txi);
0207
0208         if (ret < 0)
0209             return ret;
0210
0211         total_size += ret;
0212
0213         spin_lock_irqsave(&udc->lock_tx, flags);
0214     }
0215
0216     spin_unlock_irqrestore(&udc->lock_tx, flags);
0217     return total_size;
0218 }
0219
0220
0221 int v_tx_loop(void *data)
0222 {
0223     struct usbip_device *ud = (struct usbip_device *) data;
0224     struct vudc *udc = container_of(ud, struct vudc, ud);
0225     int ret;
0226
0227     while (!kthread_should_stop()) {
0228         if (usbip_event_happened(&udc->ud))
0229             break;
0230         ret = v_send_ret(udc);
0231         if (ret < 0) {
0232             pr_warn("v_tx exit with error %d", ret);
0233             break;
0234         }
0235         wait_event_interruptible(udc->tx_waitq,
0236                      (!list_empty(&udc->tx_queue) ||
0237                      kthread_should_stop()));
0238     }
0239
0240     return 0;
0241 }
0242
0243 /* called with spinlocks held */
0244 void v_enqueue_ret_unlink(struct vudc *udc, __u32 seqnum, __u32 status)
0245 {
0246     struct tx_item *txi;
0247     struct v_unlink *unlink;
0248
0249     txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
0250     if (!txi) {
0251         usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
0252         return;
0253     }
0254     unlink = kzalloc(sizeof(*unlink), GFP_ATOMIC);
0255     if (!unlink) {
0256         kfree(txi);
0257         usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
0258         return;
0259     }
0260
0261     unlink->seqnum = seqnum;
0262     unlink->status = status;
0263     txi->type = TX_UNLINK;
0264     txi->u = unlink;
0265
0266     list_add_tail(&txi->tx_entry, &udc->tx_queue);
0267 }
0268
0269 /* called with spinlocks held */
0270 void v_enqueue_ret_submit(struct vudc *udc, struct urbp *urb_p)
0271 {
0272     struct tx_item *txi;
0273
0274     txi = kzalloc(sizeof(*txi), GFP_ATOMIC);
0275     if (!txi) {
0276         usbip_event_add(&udc->ud, VDEV_EVENT_ERROR_MALLOC);
0277         return;
0278     }
0279
0280     txi->type = TX_SUBMIT;
0281     txi->s = urb_p;
0282
0283     list_add_tail(&txi->tx_entry, &udc->tx_queue);
0284 }