txring.c 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237
  1. /* $Id$ */
  2. /* Copyright (c) 2010 Dmitriy Gerasimov <gesser@demlabs.ru>
  3. * Copyright (c) 2010 Aaron Turner.
  4. * Copyright (c) 2023 Fred Klassen - AppNet Inc.
  5. * All rights reserved.
  6. *
  7. * Redistribution and use in source and binary forms, with or without
  8. * modification, are permitted provided that the following conditions
  9. * are met:
  10. *
  11. * 1. Redistributions of source code must retain the above copyright
  12. * notice, this list of conditions and the following disclaimer.
  13. * 2. Redistributions in binary form must reproduce the above copyright
  14. * notice, this list of conditions and the following disclaimer in the
  15. * documentation and/or other materials provided with the distribution.
  16. * 3. Neither the names of the copyright owners nor the names of its
  17. * contributors may be used to endorse or promote products derived from
  18. * this software without specific prior written permission.
  19. *
  20. * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED
  21. * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  22. * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  23. * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
  24. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  25. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
  26. * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  27. * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
  28. * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  29. * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
  30. * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  31. */
  32. #ifdef HAVE_TX_RING
  33. #include "txring.h"
  34. #include "err.h"
  35. #include "utils.h"
  36. #include <errno.h>
  37. #include <string.h>
  38. #include <sys/mman.h>
  39. #include <unistd.h>
  40. volatile int shutdown_flag = 0;
  41. int tdata_offset = TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
  42. /**
  43. * This task will call send() procedure
  44. */
  45. void *
  46. txring_send(void *arg)
  47. {
  48. int ec_send;
  49. static int total = 0;
  50. int fd_socket = (int)arg;
  51. do {
  52. /* send all buffers with TP_STATUS_SEND_REQUEST */
  53. ec_send = sendto(fd_socket, NULL, 0, MSG_DONTWAIT, (struct sockaddr *)NULL, sizeof(struct sockaddr_ll));
  54. if (ec_send > 0) {
  55. total += ec_send;
  56. dbgx(2, "Sent %d bytes (+%d bytes)", total, ec_send);
  57. } else {
  58. /* nothing to do => schedule : useful if no SMP */
  59. usleep(100);
  60. }
  61. } while (!shutdown_flag);
  62. // if(blocking) printf("end of task send()\n");
  63. // printf("end of task send(ec=%x)\n", ec_send);
  64. return (void *)ec_send;
  65. }
  66. /**
  67. * Put data in TX ring buffer and rotate it if necessary
  68. */
  69. int
  70. txring_put(txring_t *txp, const void *data, size_t length)
  71. {
  72. struct tpacket_hdr *ps_header;
  73. char *to_data;
  74. int loop = 1;
  75. int first_loop = 1;
  76. unsigned int start_index = txp->tx_index;
  77. do {
  78. ps_header = ((struct tpacket_hdr *)((void *)txp->tx_head + (txp->treq->tp_frame_size * txp->tx_index)));
  79. to_data = ((void *)ps_header) + tdata_offset;
  80. switch ((volatile uint32_t)ps_header->tp_status) {
  81. case TP_STATUS_WRONG_FORMAT:
  82. warnx("TP_STATUS_WRONG_FORMAT occuries O_o. Frame %d, pkt len %d\n", txp->tx_index, length);
  83. break;
  84. case TP_STATUS_AVAILABLE:
  85. if (length > txp->treq->tp_frame_size) {
  86. // TODO Fragment packet
  87. warnx("[!] %d bytes from %d packet truncated\n", length - txp->treq->tp_frame_size, length);
  88. length = txp->treq->tp_frame_size;
  89. }
  90. memcpy(to_data, data, length);
  91. ps_header->tp_len = length;
  92. ps_header->tp_status = TP_STATUS_SEND_REQUEST;
  93. loop = 0;
  94. break;
  95. default:
  96. dbgx(2,
  97. "TPACKET status %u at frame %d with length %d\n",
  98. ps_header->tp_status,
  99. txp->tx_index,
  100. ps_header->tp_len);
  101. usleep(0);
  102. break;
  103. }
  104. txp->tx_index++;
  105. if (txp->tx_index >= txp->treq->tp_frame_nr) {
  106. txp->tx_index = 0;
  107. first_loop = 0;
  108. }
  109. /* check if we've ran over all ring */
  110. if ((txp->tx_index == start_index) && !first_loop) {
  111. errno = ENOBUFS;
  112. return -1;
  113. }
  114. } while (loop == 1);
  115. return ps_header->tp_len;
  116. }
  117. /**
  118. * \brief Build TX ring buffer request structure
  119. *
  120. * This builds a ring buffer request structure making sure
  121. * that we have buffers big enough so that a frame which
  122. * is the size of the MTU doesn't get truncated. We also
  123. * need to structure things with minimum memory wastage
  124. */
  125. void
  126. txring_mkreq(struct tpacket_req *treq, unsigned int mtu)
  127. {
  128. unsigned int pg, bs;
  129. unsigned int s;
  130. unsigned int mult = 1;
  131. unsigned nr_blocks = 1000;
  132. bs = pg = getpagesize();
  133. s = mtu + TPACKET_HDRLEN;
  134. memset(treq, 0, sizeof(struct tpacket_req));
  135. if (bs <= s) {
  136. while (bs < s) {
  137. bs += pg;
  138. mult++;
  139. }
  140. treq->tp_block_size = bs;
  141. treq->tp_frame_size = bs / mult;
  142. treq->tp_block_nr = nr_blocks;
  143. treq->tp_frame_nr = mult * nr_blocks;
  144. } else {
  145. while ((s * (mult + 1)) <= pg) {
  146. mult++;
  147. }
  148. treq->tp_block_size = pg;
  149. treq->tp_frame_size = pg / mult;
  150. treq->tp_block_nr = nr_blocks;
  151. treq->tp_frame_nr = mult * nr_blocks;
  152. }
  153. dbgx(1,
  154. "txring: block_size=%d block_nr=%d frame_size=%d frame_nr=%d",
  155. treq->tp_block_size,
  156. treq->tp_block_nr,
  157. treq->tp_frame_size,
  158. treq->tp_frame_nr);
  159. }
  160. /**
  161. * \brief Create TX ring for socket and init indexes
  162. *
  163. * Creates our pthread for sending, currently hardcoded for priority = 20
  164. */
  165. txring_t *
  166. txring_init(int fd, unsigned int mtu)
  167. {
  168. pthread_attr_t t_attr_send;
  169. struct sched_param para_send;
  170. int mode_loss = 0;
  171. txring_t *txp;
  172. /* allocate memory for structure and fill it with different stuff*/
  173. *txp = (txring_t *)safe_malloc(sizeof(txring_t));
  174. txp->treq = (struct tpacket_req *)safe_malloc(sizeof(struct tpacket_req));
  175. txring_mkreq(txp->treq, mtu);
  176. txp->tx_size = txp->treq->tp_block_size * txp->treq->tp_block_nr;
  177. txp->tx_index = 0; /* Set index on start*/
  178. /* Set PACKET_LOSS sockoption */
  179. if (setsockopt(fd, SOL_PACKET, PACKET_LOSS, (char *)&mode_loss, sizeof(mode_loss)) < 0) {
  180. perror("setsockopt: PACKET_LOSS");
  181. return NULL;
  182. }
  183. /* Enable TX Ring */
  184. if (setsockopt(fd, SOL_PACKET, PACKET_TX_RING, (char *)txp->treq, sizeof(struct tpacket_req)) < 0) {
  185. perror("Can't setsockopt PACKET_TX_RING");
  186. return NULL;
  187. }
  188. /* mmap unswapped memory with TX ring buffer*/
  189. txp->tx_head = mmap(0, txp->tx_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
  190. if (txp->tx_head == MAP_FAILED) {
  191. perror("mmap() failed ");
  192. return NULL;
  193. }
  194. /* Start poll thread*/
  195. pthread_attr_init(&t_attr_send);
  196. pthread_attr_setschedpolicy(&t_attr_send, SCHED_RR);
  197. para_send.sched_priority = 20;
  198. pthread_attr_setschedparam(&t_attr_send, &para_send);
  199. if (pthread_create(&txp->tx_send, &t_attr_send, txring_send, (void *)fd) != 0) {
  200. perror("pthread_create() failed\n");
  201. abort();
  202. }
  203. return txp;
  204. }
  205. #endif /* HAVE_TX_RING */