1 /**************************************************************************/
2 /* CAN user space gateway for performance benchmarks */
3 /* Copyright (C) 2013, 2014 Michal Sojka, DCE, FEE, CTU Prague */
5 /**************************************************************************/
8 #include <sys/socket.h>
10 #include <sys/ioctl.h>
18 #include <linux/if_ether.h>
19 #include <linux/if_packet.h>
22 #include <arpa/inet.h>
28 #define SO_BUSY_POLL 46
31 #define FRAME_SIZE 128
32 #define BLOCK_SIZE 4096
34 #define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
38 #define STRINGIFY(val) #val
39 #define TOSTRING(val) STRINGIFY(val)
40 #define CHECK(cmd) ({ int ret = (cmd); if (ret == -1) { perror(#cmd " line " TOSTRING(__LINE__)); exit(1); }; ret; })
41 #define CHECKPTR(cmd) ({ void *ptr = (cmd); if (ptr == (void*)-1) { perror(#cmd " line " TOSTRING(__LINE__)); exit(1); }; ptr; })
44 char *devout = "can1";
45 enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
46 enum { WRITE, OUT_MMAP, OUT_SENDMMSG } out_method = WRITE;
49 bool nonblocking = false;
58 #define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
62 enum in2out (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
69 struct mmsghdr msgs[MMSG_MAX];
70 struct iovec iovecs[MMSG_MAX];
71 char bufs[MMSG_MAX][sizeof(struct can_frame)];
77 int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
84 struct mmsghdr msgs[MMSG_MAX];
85 struct iovec iovecs[MMSG_MAX];
86 char bufs[MMSG_MAX][sizeof(struct can_frame)];
96 printf("store:%d\nsend:%d\ntotal:%d\n",
97 stats.store, stats.send, stats.store + stats.send);
101 enum in2out in_read(struct in_ctx *ctx, struct can_frame *cf)
103 int ret = read(ctx->s, cf, sizeof(*cf));
104 if (nonblocking && ret == -1 && errno == EAGAIN)
106 if (ret != sizeof(*cf)) {
113 void init_read(struct in_ctx *ctx)
116 struct sockaddr_can addr;
119 s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
121 int rcvbuf = 25000; /* Limit rcvbuf to not have so big queueing latencies */
122 CHECK(setsockopt(s, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf)));
125 int flags = CHECK(fcntl(s, F_GETFL, 0));
126 CHECK(fcntl(s, F_SETFL, flags | O_NONBLOCK));
130 CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
131 &busy_poll_us, sizeof(busy_poll_us)));
134 strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name));
135 if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
140 addr.can_family = AF_CAN;
141 addr.can_ifindex = ifr.ifr_ifindex;
143 CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
146 ctx->in_fn = in_read;
149 enum in2out in_recvmmsg(struct in_ctx *ctx, struct can_frame *cf)
153 if (ctx->current >= ctx->received) {
154 ret = CHECK(recvmmsg(ctx->s, ctx->msgs, MMSG_MAX, MSG_WAITFORONE, NULL));
155 if (nonblocking && ret == -1 && errno == EAGAIN)
161 memcpy(cf, ctx->bufs[ctx->current], sizeof(struct can_frame));
164 return (ctx->current < ctx->received) ? STORE_ONLY : SEND;
167 void init_recvmmg(struct in_ctx *ctx)
172 ctx->in_fn = in_recvmmsg;
174 memset(ctx->msgs, 0, sizeof(ctx->msgs));
175 memset(ctx->iovecs, 0, sizeof(ctx->iovecs));
176 for (i = 0; i < MMSG_MAX; i++) {
177 ctx->iovecs[i].iov_base = ctx->bufs[i];
178 ctx->iovecs[i].iov_len = sizeof(struct can_frame);
179 ctx->msgs[i].msg_hdr.msg_iov = &ctx->iovecs[i];
180 ctx->msgs[i].msg_hdr.msg_iovlen = 1;
184 enum in2out in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
186 volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
189 while (hdr->tp_status == TP_STATUS_KERNEL) {
190 if (in_method != IN_MMAPBUSY) {
191 struct pollfd pfd = {.fd = ctx->s, .revents = 0,
192 .events = POLLIN|POLLRDNORM|POLLERR };
193 ret = CHECK(poll(&pfd, 1, -1));
196 //struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
198 struct can_frame *cf_mmap = (void*)hdr + hdr->tp_mac;
201 ctx->current = (ctx->current + 1) % FRAME_NR;
202 hdr = ctx->ptr + ctx->current*FRAME_SIZE;
203 return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY;
207 void init_packet_rx(struct in_ctx *ctx)
210 struct sockaddr_ll my_addr;
213 s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
216 CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
217 &busy_poll_us, sizeof(busy_poll_us)));
220 int val = TPACKET_V2;
221 CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
222 socklen_t len = sizeof(ctx->hdrlen);
223 CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
225 strncpy (ifr.ifr_name, devin, sizeof(ifr.ifr_name));
226 CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
228 my_addr.sll_family = AF_PACKET;
229 my_addr.sll_protocol = htons(ETH_P_ALL);
230 my_addr.sll_ifindex = ifr.ifr_ifindex;
232 CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
234 struct tpacket_req req = {
235 .tp_block_size = BLOCK_SIZE,
236 .tp_frame_size = FRAME_SIZE,
237 .tp_block_nr = BLOCK_NR,
238 .tp_frame_nr = FRAME_NR,
240 CHECK(setsockopt(s, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
242 ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
245 ctx->in_fn = in_packet_rx;
249 int out_write(struct out_ctx *ctx, struct can_frame *cf)
251 if (ctx->from_in == NOP)
254 int ret = write(ctx->s, cf, sizeof(*cf));
255 if (ret != sizeof(*cf)) {
262 void init_write(struct out_ctx *ctx)
265 struct sockaddr_can addr;
268 s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
270 strncpy(ifr.ifr_name, devout, sizeof(ifr.ifr_name));
271 if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
276 addr.can_family = AF_CAN;
277 addr.can_ifindex = ifr.ifr_ifindex;
279 CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
282 ctx->out_fn = out_write;
285 int out_sendmmsg(struct out_ctx *ctx, struct can_frame *cf)
287 if (ctx->from_in == NOP)
290 memcpy(ctx->bufs[ctx->current++], cf, sizeof(*cf));
292 if (ctx->from_in == SEND ||
293 ctx->current >= MMSG_MAX) {
295 for (i = 0; i < ctx->current; i += ret)
296 ret = CHECK(sendmmsg(ctx->s, &ctx->msgs[i], ctx->current - i, 0));
302 void init_sendmmsg(struct out_ctx *ctx)
307 ctx->out_fn = out_sendmmsg;
311 memset(ctx->msgs, 0, sizeof(ctx->msgs));
312 memset(ctx->iovecs, 0, sizeof(ctx->iovecs));
313 for (i = 0; i < MMSG_MAX; i++) {
314 ctx->iovecs[i].iov_base = &ctx->bufs[i];
315 ctx->iovecs[i].iov_len = sizeof(struct can_frame);
316 ctx->msgs[i].msg_hdr.msg_iov = &ctx->iovecs[i];
317 ctx->msgs[i].msg_hdr.msg_iovlen = 1;
321 int out_packet_tx(struct out_ctx *ctx, struct can_frame *cf)
323 volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
325 if (ctx->from_in == NOP) {
326 CHECK(send(ctx->s, NULL, 0, 0));
330 while (hdr->tp_status != TP_STATUS_AVAILABLE) {
331 CHECK(send(ctx->s, NULL, 0, 0));
334 //struct sockaddr_ll *addr = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
335 struct can_frame *cf_mmap = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
337 hdr->tp_len = sizeof(*cf);
338 hdr->tp_status = TP_STATUS_SEND_REQUEST;
339 ctx->current = (ctx->current + 1) % FRAME_NR;
341 if (ctx->from_in == SEND){
342 CHECK(send(ctx->s, NULL, 0, 0));
349 void init_packet_tx(struct out_ctx *ctx)
352 struct sockaddr_ll my_addr;
355 s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
357 int val = TPACKET_V2;
358 CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
359 socklen_t len = sizeof(ctx->hdrlen);
360 CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
362 strncpy (ifr.ifr_name, devout, sizeof(ifr.ifr_name));
363 CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
365 my_addr.sll_family = AF_PACKET;
366 my_addr.sll_protocol = htons(ETH_P_CAN);
367 my_addr.sll_ifindex = ifr.ifr_ifindex;
369 CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
371 struct tpacket_req req = {
372 .tp_block_size = BLOCK_SIZE,
373 .tp_frame_size = FRAME_SIZE,
374 .tp_block_nr = BLOCK_NR,
375 .tp_frame_nr = FRAME_NR,
377 CHECK(setsockopt(s, SOL_PACKET, PACKET_TX_RING, (char *)&req, sizeof(req)));
379 ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
382 ctx->out_fn = out_packet_tx;
386 void init_in(struct in_ctx *in)
388 memset(in, 0, sizeof(*in));
401 fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
406 int in(struct in_ctx *ctx, struct can_frame *cf)
408 return ctx->in_fn(ctx, cf);
411 void init_out(struct out_ctx *out)
413 memset(out, 0, sizeof(*out));
414 switch (out_method) {
415 case WRITE: init_write(out); break;
416 case OUT_SENDMMSG: init_sendmmsg(out); break;
417 case OUT_MMAP: init_packet_tx(out); break;
419 fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
424 int out(struct out_ctx *ctx, struct can_frame *cf)
426 return ctx->out_fn(ctx, cf);
438 VERBOSE("UGW started\n");
441 oc.from_in = in(&ic, &cf);
442 switch (oc.from_in) {
443 case SEND: stats.send++; break;
444 case STORE_ONLY: stats.store++; break;
452 int main(int argc, char *argv[])
456 while ((opt = getopt(argc, argv, "b:nqr:t:")) != -1) {
459 busy_poll_us = atoi(optarg);
468 if (strcmp(optarg, "read") == 0)
470 else if (strcmp(optarg, "mmap") == 0)
472 else if (strcmp(optarg, "mmapbusy") == 0)
473 in_method = IN_MMAPBUSY;
474 else if (strcmp(optarg, "mmsg") == 0)
475 in_method = IN_RECVMMSG;
477 fprintf(stderr, "Unsuported RX method: %s\n", optarg);
482 if (strcmp(optarg, "write") == 0)
484 else if (strcmp(optarg, "mmap") == 0)
485 out_method = OUT_MMAP;
486 else if (strcmp(optarg, "mmsg") == 0)
487 out_method = OUT_SENDMMSG;
489 fprintf(stderr, "Unsuported TX method: %s\n", optarg);
494 fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
501 devin = argv[optind];
503 devout = argv[optind+1];
505 signal(SIGINT, sigint);