stub_tx.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453
  1. // SPDX-License-Identifier: GPL-2.0+
  2. /*
  3. * Copyright (C) 2003-2008 Takahiro Hirofuchi
  4. */
  5. #include <linux/kthread.h>
  6. #include <linux/socket.h>
  7. #include <linux/scatterlist.h>
  8. #include "usbip_common.h"
  9. #include "stub.h"
  10. /* be in spin_lock_irqsave(&sdev->priv_lock, flags) */
  11. void stub_enqueue_ret_unlink(struct stub_device *sdev, __u32 seqnum,
  12. __u32 status)
  13. {
  14. struct stub_unlink *unlink;
  15. unlink = kzalloc(sizeof(struct stub_unlink), GFP_ATOMIC);
  16. if (!unlink) {
  17. usbip_event_add(&sdev->ud, VDEV_EVENT_ERROR_MALLOC);
  18. return;
  19. }
  20. unlink->seqnum = seqnum;
  21. unlink->status = status;
  22. list_add_tail(&unlink->list, &sdev->unlink_tx);
  23. }
  24. /**
  25. * stub_complete - completion handler of a usbip urb
  26. * @urb: pointer to the urb completed
  27. *
  28. * When a urb has completed, the USB core driver calls this function mostly in
  29. * the interrupt context. To return the result of a urb, the completed urb is
  30. * linked to the pending list of returning.
  31. *
  32. */
  33. void stub_complete(struct urb *urb)
  34. {
  35. struct stub_priv *priv = (struct stub_priv *) urb->context;
  36. struct stub_device *sdev = priv->sdev;
  37. unsigned long flags;
  38. usbip_dbg_stub_tx("complete! status %d\n", urb->status);
  39. switch (urb->status) {
  40. case 0:
  41. /* OK */
  42. break;
  43. case -ENOENT:
  44. dev_info(&urb->dev->dev,
  45. "stopped by a call to usb_kill_urb() because of cleaning up a virtual connection\n");
  46. return;
  47. case -ECONNRESET:
  48. dev_info(&urb->dev->dev,
  49. "unlinked by a call to usb_unlink_urb()\n");
  50. break;
  51. case -EPIPE:
  52. dev_info(&urb->dev->dev, "endpoint %d is stalled\n",
  53. usb_pipeendpoint(urb->pipe));
  54. break;
  55. case -ESHUTDOWN:
  56. dev_info(&urb->dev->dev, "device removed?\n");
  57. break;
  58. default:
  59. dev_info(&urb->dev->dev,
  60. "urb completion with non-zero status %d\n",
  61. urb->status);
  62. break;
  63. }
  64. /*
  65. * If the server breaks single SG request into the several URBs, the
  66. * URBs must be reassembled before sending completed URB to the vhci.
  67. * Don't wake up the tx thread until all the URBs are completed.
  68. */
  69. if (priv->sgl) {
  70. priv->completed_urbs++;
  71. /* Only save the first error status */
  72. if (urb->status && !priv->urb_status)
  73. priv->urb_status = urb->status;
  74. if (priv->completed_urbs < priv->num_urbs)
  75. return;
  76. }
  77. /* link a urb to the queue of tx. */
  78. spin_lock_irqsave(&sdev->priv_lock, flags);
  79. if (sdev->ud.tcp_socket == NULL) {
  80. usbip_dbg_stub_tx("ignore urb for closed connection\n");
  81. /* It will be freed in stub_device_cleanup_urbs(). */
  82. } else if (priv->unlinking) {
  83. stub_enqueue_ret_unlink(sdev, priv->seqnum, urb->status);
  84. stub_free_priv_and_urb(priv);
  85. } else {
  86. list_move_tail(&priv->list, &sdev->priv_tx);
  87. }
  88. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  89. /* wake up tx_thread */
  90. wake_up(&sdev->tx_waitq);
  91. }
  92. static inline void setup_base_pdu(struct usbip_header_basic *base,
  93. __u32 command, __u32 seqnum)
  94. {
  95. base->command = command;
  96. base->seqnum = seqnum;
  97. base->devid = 0;
  98. base->ep = 0;
  99. base->direction = 0;
  100. }
  101. static void setup_ret_submit_pdu(struct usbip_header *rpdu, struct urb *urb)
  102. {
  103. struct stub_priv *priv = (struct stub_priv *) urb->context;
  104. setup_base_pdu(&rpdu->base, USBIP_RET_SUBMIT, priv->seqnum);
  105. usbip_pack_pdu(rpdu, urb, USBIP_RET_SUBMIT, 1);
  106. }
  107. static void setup_ret_unlink_pdu(struct usbip_header *rpdu,
  108. struct stub_unlink *unlink)
  109. {
  110. setup_base_pdu(&rpdu->base, USBIP_RET_UNLINK, unlink->seqnum);
  111. rpdu->u.ret_unlink.status = unlink->status;
  112. }
  113. static struct stub_priv *dequeue_from_priv_tx(struct stub_device *sdev)
  114. {
  115. unsigned long flags;
  116. struct stub_priv *priv, *tmp;
  117. spin_lock_irqsave(&sdev->priv_lock, flags);
  118. list_for_each_entry_safe(priv, tmp, &sdev->priv_tx, list) {
  119. list_move_tail(&priv->list, &sdev->priv_free);
  120. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  121. return priv;
  122. }
  123. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  124. return NULL;
  125. }
  126. static int stub_send_ret_submit(struct stub_device *sdev)
  127. {
  128. unsigned long flags;
  129. struct stub_priv *priv, *tmp;
  130. struct msghdr msg;
  131. size_t txsize;
  132. size_t total_size = 0;
  133. while ((priv = dequeue_from_priv_tx(sdev)) != NULL) {
  134. struct urb *urb = priv->urbs[0];
  135. struct usbip_header pdu_header;
  136. struct usbip_iso_packet_descriptor *iso_buffer = NULL;
  137. struct kvec *iov = NULL;
  138. struct scatterlist *sg;
  139. u32 actual_length = 0;
  140. int iovnum = 0;
  141. int ret;
  142. int i;
  143. txsize = 0;
  144. memset(&pdu_header, 0, sizeof(pdu_header));
  145. memset(&msg, 0, sizeof(msg));
  146. if (urb->actual_length > 0 && !urb->transfer_buffer &&
  147. !urb->num_sgs) {
  148. dev_err(&sdev->udev->dev,
  149. "urb: actual_length %d transfer_buffer null\n",
  150. urb->actual_length);
  151. return -1;
  152. }
  153. if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS)
  154. iovnum = 2 + urb->number_of_packets;
  155. else if (usb_pipein(urb->pipe) && urb->actual_length > 0 &&
  156. urb->num_sgs)
  157. iovnum = 1 + urb->num_sgs;
  158. else if (usb_pipein(urb->pipe) && priv->sgl)
  159. iovnum = 1 + priv->num_urbs;
  160. else
  161. iovnum = 2;
  162. iov = kcalloc(iovnum, sizeof(struct kvec), GFP_KERNEL);
  163. if (!iov) {
  164. usbip_event_add(&sdev->ud, SDEV_EVENT_ERROR_MALLOC);
  165. return -1;
  166. }
  167. iovnum = 0;
  168. /* 1. setup usbip_header */
  169. setup_ret_submit_pdu(&pdu_header, urb);
  170. usbip_dbg_stub_tx("setup txdata seqnum: %d\n",
  171. pdu_header.base.seqnum);
  172. if (priv->sgl) {
  173. for (i = 0; i < priv->num_urbs; i++)
  174. actual_length += priv->urbs[i]->actual_length;
  175. pdu_header.u.ret_submit.status = priv->urb_status;
  176. pdu_header.u.ret_submit.actual_length = actual_length;
  177. }
  178. usbip_header_correct_endian(&pdu_header, 1);
  179. iov[iovnum].iov_base = &pdu_header;
  180. iov[iovnum].iov_len = sizeof(pdu_header);
  181. iovnum++;
  182. txsize += sizeof(pdu_header);
  183. /* 2. setup transfer buffer */
  184. if (usb_pipein(urb->pipe) && priv->sgl) {
  185. /* If the server split a single SG request into several
  186. * URBs because the server's HCD doesn't support SG,
  187. * reassemble the split URB buffers into a single
  188. * return command.
  189. */
  190. for (i = 0; i < priv->num_urbs; i++) {
  191. iov[iovnum].iov_base =
  192. priv->urbs[i]->transfer_buffer;
  193. iov[iovnum].iov_len =
  194. priv->urbs[i]->actual_length;
  195. iovnum++;
  196. }
  197. txsize += actual_length;
  198. } else if (usb_pipein(urb->pipe) &&
  199. usb_pipetype(urb->pipe) != PIPE_ISOCHRONOUS &&
  200. urb->actual_length > 0) {
  201. if (urb->num_sgs) {
  202. unsigned int copy = urb->actual_length;
  203. int size;
  204. for_each_sg(urb->sg, sg, urb->num_sgs, i) {
  205. if (copy == 0)
  206. break;
  207. if (copy < sg->length)
  208. size = copy;
  209. else
  210. size = sg->length;
  211. iov[iovnum].iov_base = sg_virt(sg);
  212. iov[iovnum].iov_len = size;
  213. iovnum++;
  214. copy -= size;
  215. }
  216. } else {
  217. iov[iovnum].iov_base = urb->transfer_buffer;
  218. iov[iovnum].iov_len = urb->actual_length;
  219. iovnum++;
  220. }
  221. txsize += urb->actual_length;
  222. } else if (usb_pipein(urb->pipe) &&
  223. usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
  224. /*
  225. * For isochronous packets: actual length is the sum of
  226. * the actual length of the individual, packets, but as
  227. * the packet offsets are not changed there will be
  228. * padding between the packets. To optimally use the
  229. * bandwidth the padding is not transmitted.
  230. */
  231. int i;
  232. for (i = 0; i < urb->number_of_packets; i++) {
  233. iov[iovnum].iov_base = urb->transfer_buffer +
  234. urb->iso_frame_desc[i].offset;
  235. iov[iovnum].iov_len =
  236. urb->iso_frame_desc[i].actual_length;
  237. iovnum++;
  238. txsize += urb->iso_frame_desc[i].actual_length;
  239. }
  240. if (txsize != sizeof(pdu_header) + urb->actual_length) {
  241. dev_err(&sdev->udev->dev,
  242. "actual length of urb %d does not match iso packet sizes %zu\n",
  243. urb->actual_length,
  244. txsize-sizeof(pdu_header));
  245. kfree(iov);
  246. usbip_event_add(&sdev->ud,
  247. SDEV_EVENT_ERROR_TCP);
  248. return -1;
  249. }
  250. }
  251. /* 3. setup iso_packet_descriptor */
  252. if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
  253. ssize_t len = 0;
  254. iso_buffer = usbip_alloc_iso_desc_pdu(urb, &len);
  255. if (!iso_buffer) {
  256. usbip_event_add(&sdev->ud,
  257. SDEV_EVENT_ERROR_MALLOC);
  258. kfree(iov);
  259. return -1;
  260. }
  261. iov[iovnum].iov_base = iso_buffer;
  262. iov[iovnum].iov_len = len;
  263. txsize += len;
  264. iovnum++;
  265. }
  266. ret = kernel_sendmsg(sdev->ud.tcp_socket, &msg,
  267. iov, iovnum, txsize);
  268. if (ret != txsize) {
  269. dev_err(&sdev->udev->dev,
  270. "sendmsg failed!, retval %d for %zd\n",
  271. ret, txsize);
  272. kfree(iov);
  273. kfree(iso_buffer);
  274. usbip_event_add(&sdev->ud, SDEV_EVENT_ERROR_TCP);
  275. return -1;
  276. }
  277. kfree(iov);
  278. kfree(iso_buffer);
  279. total_size += txsize;
  280. }
  281. spin_lock_irqsave(&sdev->priv_lock, flags);
  282. list_for_each_entry_safe(priv, tmp, &sdev->priv_free, list) {
  283. stub_free_priv_and_urb(priv);
  284. }
  285. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  286. return total_size;
  287. }
  288. static struct stub_unlink *dequeue_from_unlink_tx(struct stub_device *sdev)
  289. {
  290. unsigned long flags;
  291. struct stub_unlink *unlink, *tmp;
  292. spin_lock_irqsave(&sdev->priv_lock, flags);
  293. list_for_each_entry_safe(unlink, tmp, &sdev->unlink_tx, list) {
  294. list_move_tail(&unlink->list, &sdev->unlink_free);
  295. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  296. return unlink;
  297. }
  298. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  299. return NULL;
  300. }
  301. static int stub_send_ret_unlink(struct stub_device *sdev)
  302. {
  303. unsigned long flags;
  304. struct stub_unlink *unlink, *tmp;
  305. struct msghdr msg;
  306. struct kvec iov[1];
  307. size_t txsize;
  308. size_t total_size = 0;
  309. while ((unlink = dequeue_from_unlink_tx(sdev)) != NULL) {
  310. int ret;
  311. struct usbip_header pdu_header;
  312. txsize = 0;
  313. memset(&pdu_header, 0, sizeof(pdu_header));
  314. memset(&msg, 0, sizeof(msg));
  315. memset(&iov, 0, sizeof(iov));
  316. usbip_dbg_stub_tx("setup ret unlink %lu\n", unlink->seqnum);
  317. /* 1. setup usbip_header */
  318. setup_ret_unlink_pdu(&pdu_header, unlink);
  319. usbip_header_correct_endian(&pdu_header, 1);
  320. iov[0].iov_base = &pdu_header;
  321. iov[0].iov_len = sizeof(pdu_header);
  322. txsize += sizeof(pdu_header);
  323. ret = kernel_sendmsg(sdev->ud.tcp_socket, &msg, iov,
  324. 1, txsize);
  325. if (ret != txsize) {
  326. dev_err(&sdev->udev->dev,
  327. "sendmsg failed!, retval %d for %zd\n",
  328. ret, txsize);
  329. usbip_event_add(&sdev->ud, SDEV_EVENT_ERROR_TCP);
  330. return -1;
  331. }
  332. usbip_dbg_stub_tx("send txdata\n");
  333. total_size += txsize;
  334. }
  335. spin_lock_irqsave(&sdev->priv_lock, flags);
  336. list_for_each_entry_safe(unlink, tmp, &sdev->unlink_free, list) {
  337. list_del(&unlink->list);
  338. kfree(unlink);
  339. }
  340. spin_unlock_irqrestore(&sdev->priv_lock, flags);
  341. return total_size;
  342. }
  343. int stub_tx_loop(void *data)
  344. {
  345. struct usbip_device *ud = data;
  346. struct stub_device *sdev = container_of(ud, struct stub_device, ud);
  347. while (!kthread_should_stop()) {
  348. if (usbip_event_happened(ud))
  349. break;
  350. /*
  351. * send_ret_submit comes earlier than send_ret_unlink. stub_rx
  352. * looks at only priv_init queue. If the completion of a URB is
  353. * earlier than the receive of CMD_UNLINK, priv is moved to
  354. * priv_tx queue and stub_rx does not find the target priv. In
  355. * this case, vhci_rx receives the result of the submit request
  356. * and then receives the result of the unlink request. The
  357. * result of the submit is given back to the usbcore as the
  358. * completion of the unlink request. The request of the
  359. * unlink is ignored. This is ok because a driver who calls
  360. * usb_unlink_urb() understands the unlink was too late by
  361. * getting the status of the given-backed URB which has the
  362. * status of usb_submit_urb().
  363. */
  364. if (stub_send_ret_submit(sdev) < 0)
  365. break;
  366. if (stub_send_ret_unlink(sdev) < 0)
  367. break;
  368. wait_event_interruptible(sdev->tx_waitq,
  369. (!list_empty(&sdev->priv_tx) ||
  370. !list_empty(&sdev->unlink_tx) ||
  371. kthread_should_stop()));
  372. }
  373. return 0;
  374. }