txring.c 7.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241
  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 "err.h"
  34. #include "utils.h"
  35. #include "txring.h"
  36. #include <unistd.h>
  37. #include <string.h>
  38. #include <sys/mman.h>
  39. #include <errno.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,
  54. (struct sockaddr *)NULL, sizeof(struct sockaddr_ll));
  55. if (ec_send > 0) {
  56. total += ec_send;
  57. dbgx(2, "Sent %d bytes (+%d bytes)", total, ec_send);
  58. } else {
  59. /* nothing to do => schedule : useful if no SMP */
  60. usleep(100);
  61. }
  62. } while (!shutdown_flag);
  63. //if(blocking) printf("end of task send()\n");
  64. //printf("end of task send(ec=%x)\n", ec_send);
  65. return (void*) ec_send;
  66. }
  67. /**
  68. * Put data in TX ring buffer and rotate it if necessary
  69. */
  70. int
  71. txring_put(txring_t *txp, const void * data, size_t length)
  72. {
  73. struct tpacket_hdr *ps_header;
  74. char * to_data;
  75. int loop = 1;
  76. int first_loop = 1;
  77. unsigned int start_index = txp->tx_index;
  78. do {
  79. ps_header = ((struct tpacket_hdr *)((void *)txp->tx_head +
  80. (txp->treq->tp_frame_size * txp->tx_index)));
  81. to_data = ((void*) ps_header) + tdata_offset;
  82. switch ((volatile uint32_t)ps_header->tp_status) {
  83. case TP_STATUS_WRONG_FORMAT:
  84. warnx("TP_STATUS_WRONG_FORMAT occuries O_o. Frame %d, pkt len %d\n",
  85. txp->tx_index, length);
  86. break;
  87. case TP_STATUS_AVAILABLE:
  88. if (length > txp->treq->tp_frame_size) {
  89. //TODO Fragment packet
  90. warnx("[!] %d bytes from %d packet truncated\n",
  91. length-txp->treq->tp_frame_size, length);
  92. length = txp->treq->tp_frame_size;
  93. }
  94. memcpy(to_data, data, length);
  95. ps_header->tp_len = length;
  96. ps_header->tp_status = TP_STATUS_SEND_REQUEST;
  97. loop = 0;
  98. break;
  99. default:
  100. dbgx(2,"TPACKET status %u at frame %d with length %d\n",
  101. ps_header->tp_status, txp->tx_index, ps_header->tp_len);
  102. usleep(0);
  103. break;
  104. }
  105. txp->tx_index++;
  106. if (txp->tx_index >= txp->treq->tp_frame_nr) {
  107. txp->tx_index = 0;
  108. first_loop = 0;
  109. }
  110. /* check if we've ran over all ring */
  111. if ((txp->tx_index == start_index) && !first_loop) {
  112. errno = ENOBUFS;
  113. return -1;
  114. }
  115. } while(loop == 1);
  116. return ps_header->tp_len;
  117. }
  118. /**
  119. * \brief Build TX ring buffer request structure
  120. *
  121. * This builds a ring buffer request structure making sure
  122. * that we have buffers big enough so that a frame which
  123. * is the size of the MTU doesn't get truncated. We also
  124. * need to structure things with minimum memory wastage
  125. */
  126. void
  127. txring_mkreq(struct tpacket_req* treq, unsigned int mtu)
  128. {
  129. unsigned int pg,bs;
  130. unsigned int s;
  131. unsigned int mult = 1;
  132. unsigned nr_blocks = 1000;
  133. bs = pg = getpagesize();
  134. s = mtu + TPACKET_HDRLEN;
  135. memset(treq, 0, sizeof(struct tpacket_req));
  136. if (bs <= s) {
  137. while(bs < s) {
  138. bs += pg;
  139. mult++;
  140. }
  141. treq->tp_block_size = bs;
  142. treq->tp_frame_size = bs / mult;
  143. treq->tp_block_nr = nr_blocks;
  144. treq->tp_frame_nr = mult * nr_blocks;
  145. } else {
  146. while ((s * (mult + 1)) <=pg) {
  147. mult++;
  148. }
  149. treq->tp_block_size = pg;
  150. treq->tp_frame_size = pg / mult;
  151. treq->tp_block_nr = nr_blocks;
  152. treq->tp_frame_nr = mult * nr_blocks;
  153. }
  154. dbgx(1, "txring: block_size=%d block_nr=%d frame_size=%d frame_nr=%d",
  155. treq->tp_block_size, treq->tp_block_nr, treq->tp_frame_size,
  156. treq->tp_frame_nr);
  157. }
  158. /**
  159. * \brief Create TX ring for socket and init indexes
  160. *
  161. * Creates our pthread for sending, currently hardcoded for priority = 20
  162. */
  163. txring_t *
  164. txring_init(int fd, unsigned int mtu)
  165. {
  166. pthread_attr_t t_attr_send;
  167. struct sched_param para_send;
  168. int mode_loss = 0;
  169. txring_t *txp;
  170. /* allocate memory for structure and fill it with different stuff*/
  171. *txp = (txring_t *)safe_malloc(sizeof(txring_t));
  172. txp->treq = (struct tpacket_req *)safe_malloc(sizeof(struct tpacket_req));
  173. txring_mkreq(txp->treq, mtu);
  174. txp->tx_size = txp->treq->tp_block_size * txp->treq->tp_block_nr;
  175. txp->tx_index = 0; /* Set index on start*/
  176. /* Set PACKET_LOSS sockoption */
  177. if (setsockopt(fd, SOL_PACKET, PACKET_LOSS, (char *)&mode_loss,
  178. sizeof(mode_loss)) < 0) {
  179. perror("setsockopt: PACKET_LOSS");
  180. return NULL;
  181. }
  182. /* Enable TX Ring */
  183. if (setsockopt(fd, SOL_PACKET, PACKET_TX_RING, (char *)txp->treq,
  184. 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 */