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(pdu: pdup, urb, USBIP_CMD_SUBMIT, pack: 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(list: &priv->list, head: &vdev->priv_rx); |
43 | spin_unlock_irqrestore(lock: &vdev->priv_lock, flags); |
44 | return priv; |
45 | } |
46 | |
47 | spin_unlock_irqrestore(lock: &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 ; |
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(n: iovnum, size: sizeof(*iov), GFP_KERNEL); |
86 | if (!iov) { |
87 | usbip_event_add(ud: &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(pdup: &pdu_header, urb); |
96 | usbip_header_correct_endian(pdu: &pdu_header, send: 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(epd: &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, bufflen: &len); |
127 | if (!iso_buffer) { |
128 | usbip_event_add(ud: &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(sock: vdev->ud.tcp_socket, msg: &msg, vec: iov, num: iovnum, |
140 | len: txsize); |
141 | if (ret != txsize) { |
142 | pr_err("sendmsg failed!, ret=%d for %zd\n" , ret, |
143 | txsize); |
144 | usbip_event_add(ud: &vdev->ud, VDEV_EVENT_ERROR_TCP); |
145 | err = -EPIPE; |
146 | goto err_tx; |
147 | } |
148 | |
149 | kfree(objp: iov); |
150 | /* This is only for isochronous case */ |
151 | kfree(objp: 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(objp: iso_buffer); |
163 | err_iso_buffer: |
164 | kfree(objp: 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(list: &unlink->list, head: &vdev->unlink_rx); |
178 | spin_unlock_irqrestore(lock: &vdev->priv_lock, flags); |
179 | return unlink; |
180 | } |
181 | |
182 | spin_unlock_irqrestore(lock: &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 ; |
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: &pdu_header, send: 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(sock: vdev->ud.tcp_socket, msg: &msg, vec: &iov, num: 1, len: txsize); |
220 | if (ret != txsize) { |
221 | pr_err("sendmsg failed!, ret=%d for %zd\n" , ret, |
222 | txsize); |
223 | usbip_event_add(ud: &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 | |