vudc_tx.c 6.3 KB

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