1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * Copyright (C) 2015 Karol Kosik <karo9@interia.eu> |
4 | * Copyright (C) 2015-2016 Samsung Electronics |
5 | * Igor Kotrasinski <i.kotrasinsk@samsung.com> |
6 | */ |
7 | |
8 | #include <net/sock.h> |
9 | #include <linux/list.h> |
10 | #include <linux/kthread.h> |
11 | |
12 | #include "usbip_common.h" |
13 | #include "vudc.h" |
14 | |
15 | static inline void setup_base_pdu(struct usbip_header_basic *base, |
16 | __u32 command, __u32 seqnum) |
17 | { |
18 | base->command = command; |
19 | base->seqnum = seqnum; |
20 | base->devid = 0; |
21 | base->ep = 0; |
22 | base->direction = 0; |
23 | } |
24 | |
25 | static void setup_ret_submit_pdu(struct usbip_header *rpdu, struct urbp *urb_p) |
26 | { |
27 | setup_base_pdu(base: &rpdu->base, USBIP_RET_SUBMIT, seqnum: urb_p->seqnum); |
28 | usbip_pack_pdu(pdu: rpdu, urb: urb_p->urb, USBIP_RET_SUBMIT, pack: 1); |
29 | } |
30 | |
31 | static void setup_ret_unlink_pdu(struct usbip_header *rpdu, |
32 | struct v_unlink *unlink) |
33 | { |
34 | setup_base_pdu(base: &rpdu->base, USBIP_RET_UNLINK, seqnum: unlink->seqnum); |
35 | rpdu->u.ret_unlink.status = unlink->status; |
36 | } |
37 | |
38 | static int v_send_ret_unlink(struct vudc *udc, struct v_unlink *unlink) |
39 | { |
40 | struct msghdr msg; |
41 | struct kvec iov[1]; |
42 | size_t txsize; |
43 | |
44 | int ret; |
45 | struct usbip_header ; |
46 | |
47 | txsize = 0; |
48 | memset(&pdu_header, 0, sizeof(pdu_header)); |
49 | memset(&msg, 0, sizeof(msg)); |
50 | memset(&iov, 0, sizeof(iov)); |
51 | |
52 | /* 1. setup usbip_header */ |
53 | setup_ret_unlink_pdu(rpdu: &pdu_header, unlink); |
54 | usbip_header_correct_endian(pdu: &pdu_header, send: 1); |
55 | |
56 | iov[0].iov_base = &pdu_header; |
57 | iov[0].iov_len = sizeof(pdu_header); |
58 | txsize += sizeof(pdu_header); |
59 | |
60 | ret = kernel_sendmsg(sock: udc->ud.tcp_socket, msg: &msg, vec: iov, |
61 | num: 1, len: txsize); |
62 | if (ret != txsize) { |
63 | usbip_event_add(ud: &udc->ud, VUDC_EVENT_ERROR_TCP); |
64 | if (ret >= 0) |
65 | return -EPIPE; |
66 | return ret; |
67 | } |
68 | kfree(objp: unlink); |
69 | |
70 | return txsize; |
71 | } |
72 | |
73 | static int v_send_ret_submit(struct vudc *udc, struct urbp *urb_p) |
74 | { |
75 | struct urb *urb = urb_p->urb; |
76 | struct usbip_header ; |
77 | struct usbip_iso_packet_descriptor *iso_buffer = NULL; |
78 | struct kvec *iov = NULL; |
79 | int iovnum = 0; |
80 | int ret = 0; |
81 | size_t txsize; |
82 | struct msghdr msg; |
83 | |
84 | txsize = 0; |
85 | memset(&pdu_header, 0, sizeof(pdu_header)); |
86 | memset(&msg, 0, sizeof(msg)); |
87 | |
88 | if (urb->actual_length > 0 && !urb->transfer_buffer) { |
89 | dev_err(&udc->gadget.dev, |
90 | "urb: actual_length %d transfer_buffer null\n" , |
91 | urb->actual_length); |
92 | return -1; |
93 | } |
94 | |
95 | if (urb_p->type == USB_ENDPOINT_XFER_ISOC) |
96 | iovnum = 2 + urb->number_of_packets; |
97 | else |
98 | iovnum = 2; |
99 | |
100 | iov = kcalloc(n: iovnum, size: sizeof(*iov), GFP_KERNEL); |
101 | if (!iov) { |
102 | usbip_event_add(ud: &udc->ud, VUDC_EVENT_ERROR_MALLOC); |
103 | ret = -ENOMEM; |
104 | goto out; |
105 | } |
106 | iovnum = 0; |
107 | |
108 | /* 1. setup usbip_header */ |
109 | setup_ret_submit_pdu(rpdu: &pdu_header, urb_p); |
110 | usbip_dbg_stub_tx("setup txdata seqnum: %d\n" , |
111 | pdu_header.base.seqnum); |
112 | usbip_header_correct_endian(pdu: &pdu_header, send: 1); |
113 | |
114 | iov[iovnum].iov_base = &pdu_header; |
115 | iov[iovnum].iov_len = sizeof(pdu_header); |
116 | iovnum++; |
117 | txsize += sizeof(pdu_header); |
118 | |
119 | /* 2. setup transfer buffer */ |
120 | if (urb_p->type != USB_ENDPOINT_XFER_ISOC && |
121 | usb_pipein(urb->pipe) && urb->actual_length > 0) { |
122 | iov[iovnum].iov_base = urb->transfer_buffer; |
123 | iov[iovnum].iov_len = urb->actual_length; |
124 | iovnum++; |
125 | txsize += urb->actual_length; |
126 | } else if (urb_p->type == USB_ENDPOINT_XFER_ISOC && |
127 | usb_pipein(urb->pipe)) { |
128 | /* FIXME - copypasted from stub_tx, refactor */ |
129 | int i; |
130 | |
131 | for (i = 0; i < urb->number_of_packets; i++) { |
132 | iov[iovnum].iov_base = urb->transfer_buffer + |
133 | urb->iso_frame_desc[i].offset; |
134 | iov[iovnum].iov_len = |
135 | urb->iso_frame_desc[i].actual_length; |
136 | iovnum++; |
137 | txsize += urb->iso_frame_desc[i].actual_length; |
138 | } |
139 | |
140 | if (txsize != sizeof(pdu_header) + urb->actual_length) { |
141 | usbip_event_add(ud: &udc->ud, VUDC_EVENT_ERROR_TCP); |
142 | ret = -EPIPE; |
143 | goto out; |
144 | } |
145 | } |
146 | /* else - no buffer to send */ |
147 | |
148 | /* 3. setup iso_packet_descriptor */ |
149 | if (urb_p->type == USB_ENDPOINT_XFER_ISOC) { |
150 | ssize_t len = 0; |
151 | |
152 | iso_buffer = usbip_alloc_iso_desc_pdu(urb, bufflen: &len); |
153 | if (!iso_buffer) { |
154 | usbip_event_add(ud: &udc->ud, |
155 | VUDC_EVENT_ERROR_MALLOC); |
156 | ret = -ENOMEM; |
157 | goto out; |
158 | } |
159 | |
160 | iov[iovnum].iov_base = iso_buffer; |
161 | iov[iovnum].iov_len = len; |
162 | txsize += len; |
163 | iovnum++; |
164 | } |
165 | |
166 | ret = kernel_sendmsg(sock: udc->ud.tcp_socket, msg: &msg, |
167 | vec: iov, num: iovnum, len: txsize); |
168 | if (ret != txsize) { |
169 | usbip_event_add(ud: &udc->ud, VUDC_EVENT_ERROR_TCP); |
170 | if (ret >= 0) |
171 | ret = -EPIPE; |
172 | goto out; |
173 | } |
174 | |
175 | out: |
176 | kfree(objp: iov); |
177 | kfree(objp: iso_buffer); |
178 | free_urbp_and_urb(urb_p); |
179 | if (ret < 0) |
180 | return ret; |
181 | return txsize; |
182 | } |
183 | |
184 | static int v_send_ret(struct vudc *udc) |
185 | { |
186 | unsigned long flags; |
187 | struct tx_item *txi; |
188 | size_t total_size = 0; |
189 | int ret = 0; |
190 | |
191 | spin_lock_irqsave(&udc->lock_tx, flags); |
192 | while (!list_empty(head: &udc->tx_queue)) { |
193 | txi = list_first_entry(&udc->tx_queue, struct tx_item, |
194 | tx_entry); |
195 | list_del(entry: &txi->tx_entry); |
196 | spin_unlock_irqrestore(lock: &udc->lock_tx, flags); |
197 | |
198 | switch (txi->type) { |
199 | case TX_SUBMIT: |
200 | ret = v_send_ret_submit(udc, urb_p: txi->s); |
201 | break; |
202 | case TX_UNLINK: |
203 | ret = v_send_ret_unlink(udc, unlink: txi->u); |
204 | break; |
205 | } |
206 | kfree(objp: txi); |
207 | |
208 | if (ret < 0) |
209 | return ret; |
210 | |
211 | total_size += ret; |
212 | |
213 | spin_lock_irqsave(&udc->lock_tx, flags); |
214 | } |
215 | |
216 | spin_unlock_irqrestore(lock: &udc->lock_tx, flags); |
217 | return total_size; |
218 | } |
219 | |
220 | |
221 | int v_tx_loop(void *data) |
222 | { |
223 | struct usbip_device *ud = (struct usbip_device *) data; |
224 | struct vudc *udc = container_of(ud, struct vudc, ud); |
225 | int ret; |
226 | |
227 | while (!kthread_should_stop()) { |
228 | if (usbip_event_happened(ud: &udc->ud)) |
229 | break; |
230 | ret = v_send_ret(udc); |
231 | if (ret < 0) { |
232 | pr_warn("v_tx exit with error %d" , ret); |
233 | break; |
234 | } |
235 | wait_event_interruptible(udc->tx_waitq, |
236 | (!list_empty(&udc->tx_queue) || |
237 | kthread_should_stop())); |
238 | } |
239 | |
240 | return 0; |
241 | } |
242 | |
243 | /* called with spinlocks held */ |
244 | void v_enqueue_ret_unlink(struct vudc *udc, __u32 seqnum, __u32 status) |
245 | { |
246 | struct tx_item *txi; |
247 | struct v_unlink *unlink; |
248 | |
249 | txi = kzalloc(size: sizeof(*txi), GFP_ATOMIC); |
250 | if (!txi) { |
251 | usbip_event_add(ud: &udc->ud, VDEV_EVENT_ERROR_MALLOC); |
252 | return; |
253 | } |
254 | unlink = kzalloc(size: sizeof(*unlink), GFP_ATOMIC); |
255 | if (!unlink) { |
256 | kfree(objp: txi); |
257 | usbip_event_add(ud: &udc->ud, VDEV_EVENT_ERROR_MALLOC); |
258 | return; |
259 | } |
260 | |
261 | unlink->seqnum = seqnum; |
262 | unlink->status = status; |
263 | txi->type = TX_UNLINK; |
264 | txi->u = unlink; |
265 | |
266 | list_add_tail(new: &txi->tx_entry, head: &udc->tx_queue); |
267 | } |
268 | |
269 | /* called with spinlocks held */ |
270 | void v_enqueue_ret_submit(struct vudc *udc, struct urbp *urb_p) |
271 | { |
272 | struct tx_item *txi; |
273 | |
274 | txi = kzalloc(size: sizeof(*txi), GFP_ATOMIC); |
275 | if (!txi) { |
276 | usbip_event_add(ud: &udc->ud, VDEV_EVENT_ERROR_MALLOC); |
277 | return; |
278 | } |
279 | |
280 | txi->type = TX_SUBMIT; |
281 | txi->s = urb_p; |
282 | |
283 | list_add_tail(new: &txi->tx_entry, head: &udc->tx_queue); |
284 | } |
285 | |