ugw/ugw.c

   1 /**************************************************************************/
   2 /* CAN user space gateway for performance benchmarks                      */
   3 /* Copyright (C) 2013, 2014 Michal Sojka, DCE, FEE, CTU Prague            */
   4 /* License: GPLv2                                                         */
   5 /**************************************************************************/
   6
   7 #define _GNU_SOURCE
   8 #include <sys/socket.h>
   9 #include <linux/can.h>
  10 #include <sys/ioctl.h>
  11 #include <net/if.h>
  12 #include <string.h>
  13 #include <unistd.h>
  14 #include <stdlib.h>
  15 #include <stdio.h>
  16 #include <time.h>
  17 #include <stdbool.h>
  18 #include <linux/if_ether.h>
  19 #include <linux/if_packet.h>
  20 #include <poll.h>
  21 #include <sys/mman.h>
  22 #include <arpa/inet.h>
  23 #include <fcntl.h>
  24 #include <errno.h>
  25 #include <signal.h>
  26
  27 #ifndef SO_BUSY_POLL
  28 #define SO_BUSY_POLL 46
  29 #endif
  30
  31 #define FRAME_SIZE 128
  32 #define BLOCK_SIZE 4096
  33 #define BLOCK_NR 2
  34 #define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
  35
  36 #define MMSG_MAX 64
  37
  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; })
  42
  43 char *devin = "can0";
  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;
  47 bool quiet = false;
  48 int busy_poll_us = 0;
  49 bool nonblocking = false;
  50
  51 enum in2out {
  52         STORE_ONLY,
  53         SEND,
  54         NOP,
  55 };
  56
  57
  58 #define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
  59
  60 struct in_ctx {
  61         int s;
  62         enum in2out (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
  63         /* mmap */
  64         void *ptr;
  65         int hdrlen;
  66         int current;
  67         /* mmsg */
  68         int received;
  69         struct mmsghdr msgs[MMSG_MAX];
  70         struct iovec iovecs[MMSG_MAX];
  71         char bufs[MMSG_MAX][sizeof(struct can_frame)];
  72 };
  73
  74 struct out_ctx {
  75         int s;
  76         enum in2out from_in;
  77         int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
  78         /* mmap */
  79         void *ptr;
  80         int hdrlen;
  81         int current;
  82         /* mmsg */
  83         int stored;
  84         struct mmsghdr msgs[MMSG_MAX];
  85         struct iovec iovecs[MMSG_MAX];
  86         char bufs[MMSG_MAX][sizeof(struct can_frame)];
  87 };
  88
  89 struct stats {
  90         int store;
  91         int send;
  92 } stats;
  93
  94 void sigint(int v)
  95 {
  96         printf("store:%d\nsend:%d\ntotal:%d\n",
  97                stats.store, stats.send, stats.store + stats.send);
  98         exit(0);
  99 }
 100
 101 enum in2out in_read(struct in_ctx *ctx, struct can_frame *cf)
 102 {
 103         int ret = read(ctx->s, cf, sizeof(*cf));
 104         if (nonblocking && ret == -1 && errno == EAGAIN)
 105                 return NOP;
 106         if (ret != sizeof(*cf)) {
 107                 perror("read");
 108                 exit(1);
 109         }
 110         return STORE_ONLY;
 111 }
 112
 113 void init_read(struct in_ctx *ctx)
 114 {
 115         int s;
 116         struct sockaddr_can addr;
 117         struct ifreq ifr;
 118
 119         s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
 120
 121         int rcvbuf = 25000;     /* Limit rcvbuf to not have so big queueing latencies */
 122         CHECK(setsockopt(s, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf)));
 123
 124         if (nonblocking) {
 125                 int flags = CHECK(fcntl(s, F_GETFL, 0));
 126                 CHECK(fcntl(s, F_SETFL, flags | O_NONBLOCK));
 127         }
 128
 129         if (busy_poll_us) {
 130                 CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
 131                                  &busy_poll_us, sizeof(busy_poll_us)));
 132         }
 133
 134         strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name));
 135         if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
 136                 perror(devin);
 137                 exit(1);
 138         }
 139
 140         addr.can_family = AF_CAN;
 141         addr.can_ifindex = ifr.ifr_ifindex;
 142
 143         CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
 144
 145         ctx->s = s;
 146         ctx->in_fn = in_read;
 147 }
 148
 149 enum in2out in_recvmmsg(struct in_ctx *ctx, struct can_frame *cf)
 150 {
 151         int ret;
 152
 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)
 156                         return NOP;
 157                 ctx->received = ret;
 158                 ctx->current = 0;
 159         }
 160
 161         memcpy(cf, ctx->bufs[ctx->current], sizeof(struct can_frame));
 162         ctx->current++;
 163
 164         return (ctx->current < ctx->received) ? STORE_ONLY : SEND;
 165 }
 166
 167 void init_recvmmg(struct in_ctx *ctx)
 168 {
 169         int i;
 170
 171         init_read(ctx);
 172         ctx->in_fn = in_recvmmsg;
 173         ctx->current = 0;
 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;
 181         }
 182 }
 183
 184 enum in2out in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
 185 {
 186         volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
 187         int ret = -1;
 188
 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));
 194                 }
 195         }
 196         //struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
 197         (void)ret;
 198         struct can_frame *cf_mmap = (void*)hdr + hdr->tp_mac;
 199         *cf = *cf_mmap;
 200         hdr->tp_status = 0;
 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;
 204 }
 205
 206
 207 void init_packet_rx(struct in_ctx *ctx)
 208 {
 209         int s;
 210         struct sockaddr_ll my_addr;
 211         struct ifreq ifr;
 212
 213         s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
 214
 215         if (busy_poll_us) {
 216                 CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
 217                                  &busy_poll_us, sizeof(busy_poll_us)));
 218         }
 219
 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));
 224
 225         strncpy (ifr.ifr_name, devin, sizeof(ifr.ifr_name));
 226         CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
 227
 228         my_addr.sll_family = AF_PACKET;
 229         my_addr.sll_protocol = htons(ETH_P_ALL);
 230         my_addr.sll_ifindex =  ifr.ifr_ifindex;
 231
 232         CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
 233
 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,
 239         };
 240         CHECK(setsockopt(s, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
 241
 242         ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
 243
 244         ctx->s = s;
 245         ctx->in_fn = in_packet_rx;
 246         ctx->current = 0;
 247 }
 248
 249 int out_write(struct out_ctx *ctx, struct can_frame *cf)
 250 {
 251         if (ctx->from_in == NOP)
 252                 return 0;
 253
 254         int ret = write(ctx->s, cf, sizeof(*cf));
 255         if (ret != sizeof(*cf)) {
 256                 perror("write");
 257                 exit(1);
 258         }
 259         return 0;
 260 }
 261
 262 void init_write(struct out_ctx *ctx)
 263 {
 264         int s;
 265         struct sockaddr_can addr;
 266         struct ifreq ifr;
 267
 268         s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
 269
 270         strncpy(ifr.ifr_name, devout, sizeof(ifr.ifr_name));
 271         if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
 272                 perror(devout);
 273                 exit(1);
 274         }
 275
 276         addr.can_family = AF_CAN;
 277         addr.can_ifindex = ifr.ifr_ifindex;
 278
 279         CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
 280
 281         ctx->s = s;
 282         ctx->out_fn = out_write;
 283 }
 284
 285 int out_sendmmsg(struct out_ctx *ctx, struct can_frame *cf)
 286 {
 287         if (ctx->from_in == NOP)
 288                 return 0;
 289
 290         memcpy(ctx->bufs[ctx->current++], cf, sizeof(*cf));
 291
 292         if (ctx->from_in == SEND ||
 293             ctx->current >= MMSG_MAX) {
 294                 int ret, i;
 295                 for (i = 0; i < ctx->current; i += ret)
 296                         ret = CHECK(sendmmsg(ctx->s, &ctx->msgs[i], ctx->current - i, 0));
 297                 ctx->current = 0;
 298         }
 299         return 0;
 300 }
 301
 302 void init_sendmmsg(struct out_ctx *ctx)
 303 {
 304         int i;
 305
 306         init_write(ctx);
 307         ctx->out_fn = out_sendmmsg;
 308         ctx->current = 0;
 309         ctx->stored = 0;
 310
 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;
 318         }
 319 }
 320
 321 int out_packet_tx(struct out_ctx *ctx, struct can_frame *cf)
 322 {
 323         volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
 324
 325         if (ctx->from_in == NOP) {
 326                 CHECK(send(ctx->s, NULL, 0, 0));
 327                 return 0;
 328         }
 329
 330         while (hdr->tp_status != TP_STATUS_AVAILABLE) {
 331                 CHECK(send(ctx->s, NULL, 0, 0));
 332         }
 333
 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);
 336         *cf_mmap = *cf;
 337         hdr->tp_len = sizeof(*cf);
 338         hdr->tp_status = TP_STATUS_SEND_REQUEST;
 339         ctx->current = (ctx->current + 1) % FRAME_NR;
 340
 341         if (ctx->from_in == SEND){
 342                 CHECK(send(ctx->s, NULL, 0, 0));
 343                 return 0;
 344         }
 345         return 0;
 346 }
 347
 348
 349 void init_packet_tx(struct out_ctx *ctx)
 350 {
 351         int s;
 352         struct sockaddr_ll my_addr;
 353         struct ifreq ifr;
 354
 355         s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
 356
 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));
 361
 362         strncpy (ifr.ifr_name, devout, sizeof(ifr.ifr_name));
 363         CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
 364
 365         my_addr.sll_family = AF_PACKET;
 366         my_addr.sll_protocol = htons(ETH_P_CAN);
 367         my_addr.sll_ifindex =  ifr.ifr_ifindex;
 368
 369         CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
 370
 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,
 376         };
 377         CHECK(setsockopt(s, SOL_PACKET, PACKET_TX_RING, (char *)&req, sizeof(req)));
 378
 379         ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
 380
 381         ctx->s = s;
 382         ctx->out_fn = out_packet_tx;
 383         ctx->current = 0;
 384 }
 385
 386 void init_in(struct in_ctx *in)
 387 {
 388         memset(in, 0, sizeof(*in));
 389         switch (in_method) {
 390         case IN_READ:
 391                 init_read(in);
 392                 break;
 393         case IN_RECVMMSG:
 394                 init_recvmmg(in);
 395                 break;
 396         case IN_MMAP:
 397         case IN_MMAPBUSY:
 398                 init_packet_rx(in);
 399                 break;
 400         default:
 401                 fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
 402                 exit(1);
 403         }
 404 }
 405
 406 int in(struct in_ctx *ctx, struct can_frame *cf)
 407 {
 408         return ctx->in_fn(ctx, cf);
 409 }
 410
 411 void init_out(struct out_ctx *out)
 412 {
 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;
 418         default:
 419                 fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
 420                 exit(1);
 421         }
 422 }
 423
 424 int out(struct out_ctx *ctx, struct can_frame *cf)
 425 {
 426         return ctx->out_fn(ctx, cf);
 427 }
 428
 429 void gw()
 430 {
 431         struct in_ctx    ic;
 432         struct out_ctx   oc;
 433         struct can_frame cf;
 434
 435         init_in(&ic);
 436         init_out(&oc);
 437
 438         VERBOSE("UGW started\n");
 439
 440         while (1) {
 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;
 445                 case NOP:                       break;
 446                 }
 447                 out(&oc, &cf);
 448         }
 449 }
 450
 451
 452 int main(int argc, char *argv[])
 453 {
 454         int opt;
 455
 456         while ((opt = getopt(argc, argv, "b:nqr:t:")) != -1) {
 457                 switch (opt) {
 458                 case 'b':
 459                         busy_poll_us = atoi(optarg);
 460                         break;
 461                 case 'n':
 462                         nonblocking = true;
 463                         break;
 464                 case 'q':
 465                         quiet = true;
 466                         break;
 467                 case 'r':
 468                         if (strcmp(optarg, "read") == 0)
 469                                 in_method = IN_READ;
 470                         else if (strcmp(optarg, "mmap") == 0)
 471                                 in_method = IN_MMAP;
 472                         else if (strcmp(optarg, "mmapbusy") == 0)
 473                                 in_method = IN_MMAPBUSY;
 474                         else if (strcmp(optarg, "mmsg") == 0)
 475                                 in_method = IN_RECVMMSG;
 476                         else {
 477                                 fprintf(stderr, "Unsuported RX method: %s\n", optarg);
 478                                 exit(1);
 479                         }
 480                         break;
 481                 case 't':
 482                         if (strcmp(optarg, "write") == 0)
 483                                 out_method = WRITE;
 484                         else if (strcmp(optarg, "mmap") == 0)
 485                                 out_method = OUT_MMAP;
 486                         else if (strcmp(optarg, "mmsg") == 0)
 487                                 out_method = OUT_SENDMMSG;
 488                         else {
 489                                 fprintf(stderr, "Unsuported TX method: %s\n", optarg);
 490                                 exit(1);
 491                         }
 492                         break;
 493                 default: /* '?' */
 494                         fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
 495                                 argv[0]);
 496                         exit(EXIT_FAILURE);
 497                 }
 498         }
 499
 500         if (optind < argc)
 501                 devin  = argv[optind];
 502         if (optind+1 < argc)
 503                 devout = argv[optind+1];
 504
 505         signal(SIGINT, sigint);
 506         gw();
 507
 508         return 0;
 509 }