Psyllid  v1.12.4
Project 8 Data Acquisisition Software
test_tpacket_v3.cc
Go to the documentation of this file.
1 /*
2  * test_tpacket_v3.cc
3  *
4  * Created on: Sep 2, 2016
5  * Author: nsoblath
6  */
7 
8 /* Written from scratch, but kernel-to-user space API usage
9  * dissected from lolpcap:
10  * Copyright 2011, Chetan Loke <loke.chetan@gmail.com>
11  * License: GPL, version 2.0
12  */
13 
14 #include <stdio.h>
15 #include <stdlib.h>
16 #include <stdint.h>
17 #include <string.h>
18 #include <assert.h>
19 #include <net/if.h>
20 #include <arpa/inet.h>
21 #include <netdb.h>
22 #include <poll.h>
23 #include <unistd.h>
24 #include <signal.h>
25 #include <inttypes.h>
26 #include <sys/socket.h>
27 #include <sys/mman.h>
28 #include <linux/if_packet.h>
29 #include <linux/if_ether.h>
30 #include <linux/ip.h>
31 
32 #ifndef likely
33 # define likely(x) __builtin_expect(!!(x), 1)
34 #endif
35 #ifndef unlikely
36 # define unlikely(x) __builtin_expect(!!(x), 0)
37 #endif
38 
39 struct block_desc {
40  uint32_t version;
41  uint32_t offset_to_priv;
42  tpacket_hdr_v1 h1;
43 };
44 
45 struct ring {
46  iovec *rd;
47  uint8_t *map;
48  tpacket_req3 req;
49 };
50 
51 static unsigned long packets_total = 0, bytes_total = 0;
52 static sig_atomic_t sigint = 0;
53 
54 static void sighandler(int /*num*/)
55 {
56  sigint = 1;
57 }
58 
59 static int setup_socket(ring *ring, char *netdev)
60 {
61  unsigned i;
62  int err, fd, v = TPACKET_V3;
63  sockaddr_ll ll;
64  unsigned int blocksiz = 1 << 22, framesiz = 1 << 11;
65  unsigned int blocknum = 64;
66 
67  fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
68  if (fd < 0) {
69  perror("socket");
70  exit(1);
71  }
72 
73  err = setsockopt(fd, SOL_PACKET, PACKET_VERSION, &v, sizeof(v));
74  if (err < 0) {
75  perror("setsockopt");
76  exit(1);
77  }
78 
79  memset(&ring->req, 0, sizeof(ring->req));
80  ring->req.tp_block_size = blocksiz;
81  ring->req.tp_frame_size = framesiz;
82  ring->req.tp_block_nr = blocknum;
83  ring->req.tp_frame_nr = (blocksiz * blocknum) / framesiz;
84  ring->req.tp_retire_blk_tov = 60;
85  ring->req.tp_feature_req_word = TP_FT_REQ_FILL_RXHASH;
86 
87  err = setsockopt(fd, SOL_PACKET, PACKET_RX_RING, &ring->req,
88  sizeof(ring->req));
89  if (err < 0) {
90  perror("setsockopt");
91  exit(1);
92  }
93 
94  ring->map = (uint8_t*)mmap(NULL, ring->req.tp_block_size * ring->req.tp_block_nr,
95  PROT_READ | PROT_WRITE, MAP_SHARED | MAP_LOCKED, fd, 0);
96  if (ring->map == MAP_FAILED) {
97  perror("mmap");
98  exit(1);
99  }
100 
101  ring->rd = (iovec*)malloc(ring->req.tp_block_nr * sizeof(*ring->rd));
102  assert(ring->rd);
103  for (i = 0; i < ring->req.tp_block_nr; ++i) {
104  ring->rd[i].iov_base = ring->map + (i * ring->req.tp_block_size);
105  ring->rd[i].iov_len = ring->req.tp_block_size;
106  }
107 
108  memset(&ll, 0, sizeof(ll));
109  ll.sll_family = PF_PACKET;
110  ll.sll_protocol = htons(ETH_P_ALL);
111  ll.sll_ifindex = if_nametoindex(netdev);
112  ll.sll_hatype = 0;
113  ll.sll_pkttype = 0;
114  ll.sll_halen = 0;
115 
116  err = bind(fd, (sockaddr *) &ll, sizeof(ll));
117  if (err < 0) {
118  perror("bind");
119  exit(1);
120  }
121 
122  return fd;
123 }
124 
125 static void display(tpacket3_hdr *ppd)
126 {
127  ethhdr *eth = (ethhdr *) ((uint8_t *) ppd + ppd->tp_mac);
128  iphdr *ip = (iphdr *) ((uint8_t *) eth + ETH_HLEN);
129 
130  //printf("packet id: %d, ", eth->h_proto);
131  if (eth->h_proto == htons(ETH_P_IP)) {
132  sockaddr_in ss, sd;
133  char sbuff[NI_MAXHOST], dbuff[NI_MAXHOST];
134 
135  memset(&ss, 0, sizeof(ss));
136  ss.sin_family = PF_INET;
137  ss.sin_addr.s_addr = ip->saddr;
138  getnameinfo((sockaddr *) &ss, sizeof(ss),
139  sbuff, sizeof(sbuff), NULL, 0, NI_NUMERICHOST);
140 
141  memset(&sd, 0, sizeof(sd));
142  sd.sin_family = PF_INET;
143  sd.sin_addr.s_addr = ip->daddr;
144  getnameinfo((sockaddr *) &sd, sizeof(sd),
145  dbuff, sizeof(dbuff), NULL, 0, NI_NUMERICHOST);
146 
147  if( packets_total % 1000 == 0 ) printf("packet %lu; bytes read: %lu; %s -> %s\n", packets_total, bytes_total, sbuff, dbuff);
148  }
149 
150  //printf("rxhash: 0x%x\n", ppd->hv1.tp_rxhash);
151 
152  //for (int i=0; i<50; ++i)
153  //{
154  // printf("%02X", ((char*)ppd)[i]);
155  //}
156  //printf("\n");
157 }
158 
159 static void walk_block(block_desc *pbd, const int /*block_num*/)
160 {
161  int num_pkts = pbd->h1.num_pkts, i;
162  unsigned long bytes = 0;
163  tpacket3_hdr *ppd;
164 
165  ppd = (tpacket3_hdr *) ((uint8_t *) pbd +
166  pbd->h1.offset_to_first_pkt);
167  for (i = 0; i < num_pkts; ++i) {
168  bytes += ppd->tp_snaplen;
169  display(ppd);
170 
171  ppd = (tpacket3_hdr *) ((uint8_t *) ppd +
172  ppd->tp_next_offset);
173  }
174 
175  packets_total += num_pkts;
176  bytes_total += bytes;
177 }
178 
179 static void flush_block(block_desc *pbd)
180 {
181  pbd->h1.block_status = TP_STATUS_KERNEL;
182 }
183 
184 static void teardown_socket(ring *ring, int fd)
185 {
186  munmap(ring->map, ring->req.tp_block_size * ring->req.tp_block_nr);
187  free(ring->rd);
188  close(fd);
189 }
190 
191 int main(int argc, char **argp)
192 {
193  int fd, err;
194  socklen_t len;
195  ring ring;
196  pollfd pfd;
197  unsigned int block_num = 0, blocks = 64;
198  block_desc *pbd;
199  tpacket_stats_v3 stats;
200 
201  if (argc != 2) {
202  fprintf(stderr, "Usage: %s INTERFACE\n", argp[0]);
203  return EXIT_FAILURE;
204  }
205 
206  signal(SIGINT, sighandler);
207 
208  printf("setting up socket\n");
209  memset(&ring, 0, sizeof(ring));
210  fd = setup_socket(&ring, argp[argc - 1]);
211  assert(fd > 0);
212 
213  memset(&pfd, 0, sizeof(pfd));
214  pfd.fd = fd;
215  pfd.events = POLLIN | POLLERR;
216  pfd.revents = 0;
217 
218  printf("waiting for packets\n");
219 
220  while (likely(!sigint)) {
221  pbd = (block_desc *) ring.rd[block_num].iov_base;
222 
223  if ((pbd->h1.block_status & TP_STATUS_USER) == 0) {
224  poll(&pfd, 1, -1);
225  continue;
226  }
227 
228  walk_block(pbd, block_num);
229  flush_block(pbd);
230  block_num = (block_num + 1) % blocks;
231  //if (packets_total > 50) raise(SIGINT);
232  }
233 
234  len = sizeof(stats);
235  err = getsockopt(fd, SOL_PACKET, PACKET_STATISTICS, &stats, &len);
236  if (err < 0) {
237  perror("getsockopt");
238  exit(1);
239  }
240 
241  fflush(stdout);
242  printf("\nReceived %u packets, %lu bytes, %u dropped, freeze_q_cnt: %u\n",
243  stats.tp_packets, bytes_total, stats.tp_drops,
244  stats.tp_freeze_q_cnt);
245 
246  teardown_socket(&ring, fd);
247  return 0;
248 }
249 
250 
static unsigned long packets_total
static unsigned long bytes_total
static sig_atomic_t sigint
static int setup_socket(ring *ring, char *netdev)
static void display(tpacket3_hdr *ppd)
uint32_t offset_to_priv
iovec * rd
int main(int argc, char **argp)
static void sighandler(int)
static void walk_block(block_desc *pbd, const int)
uint8_t * map
#define likely(x)
static void teardown_socket(ring *ring, int fd)
static void flush_block(block_desc *pbd)
tpacket_hdr_v1 h1
uint32_t version
tpacket_req3 req