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 STRINGIFY(val) #val
  37 #define TOSTRING(val) STRINGIFY(val)
  38 #define CHECK(cmd) ({ int ret = (cmd); if (ret == -1) { perror(#cmd " line " TOSTRING(__LINE__)); exit(1); }; ret; })
  39 #define CHECKPTR(cmd) ({ void *ptr = (cmd); if (ptr == (void*)-1) { perror(#cmd " line " TOSTRING(__LINE__)); exit(1); }; ptr; })
  40
  41 char *devin = "can0";
  42 char *devout = "can1";
  43 enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
  44 enum { WRITE, OUT_MMAP } out_method = WRITE;
  45 bool quiet = false;
  46 int busy_poll_us = 0;
  47 bool nonblocking = false;
  48
  49 enum in2out {
  50         STORE_ONLY,
  51         SEND,
  52         NOP,
  53 };
  54
  55
  56 #define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
  57
  58 struct in_ctx {
  59         int s;
  60         void *ptr;
  61         int hdrlen;
  62         int current;
  63         enum in2out (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
  64 };
  65
  66 struct out_ctx {
  67         int s;
  68         enum in2out from_in;
  69         void *ptr;
  70         int hdrlen;
  71         int current;
  72         int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
  73 };
  74
  75 struct stats {
  76         int store;
  77         int send;
  78 } stats;
  79
  80 void sigint(int v)
  81 {
  82         printf("store:%d\nsend:%d\ntotal:%d\n",
  83                stats.store, stats.send, stats.store + stats.send);
  84         exit(0);
  85 }
  86
  87 enum in2out in_read(struct in_ctx *ctx, struct can_frame *cf)
  88 {
  89         int ret = read(ctx->s, cf, sizeof(*cf));
  90         if (nonblocking && ret == -1 && errno == EAGAIN)
  91                 return NOP;
  92         if (ret != sizeof(*cf)) {
  93                 perror("read");
  94                 exit(1);
  95         }
  96         return STORE_ONLY;
  97 }
  98
  99 void init_read(struct in_ctx *ctx)
 100 {
 101         int s;
 102         struct sockaddr_can addr;
 103         struct ifreq ifr;
 104
 105         s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
 106
 107         int rcvbuf = 25000;     /* Limit rcvbuf to not have so big queueing latencies */
 108         CHECK(setsockopt(s, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf)));
 109
 110         if (nonblocking) {
 111                 int flags = CHECK(fcntl(s, F_GETFL, 0));
 112                 CHECK(fcntl(s, F_SETFL, flags | O_NONBLOCK));
 113         }
 114
 115         if (busy_poll_us) {
 116                 CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
 117                                  &busy_poll_us, sizeof(busy_poll_us)));
 118         }
 119
 120         strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name));
 121         if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
 122                 perror(devin);
 123                 exit(1);
 124         }
 125
 126         addr.can_family = AF_CAN;
 127         addr.can_ifindex = ifr.ifr_ifindex;
 128
 129         CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
 130
 131         ctx->s = s;
 132         ctx->in_fn = in_read;
 133 }
 134
 135 enum in2out in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
 136 {
 137         volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
 138         int ret = -1;
 139
 140         while (hdr->tp_status == TP_STATUS_KERNEL) {
 141                 if (in_method != IN_MMAPBUSY) {
 142                         struct pollfd pfd = {.fd = ctx->s, .revents = 0,
 143                                              .events = POLLIN|POLLRDNORM|POLLERR };
 144                         ret = CHECK(poll(&pfd, 1, -1));
 145                 }
 146         }
 147         //struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
 148         (void)ret;
 149         struct can_frame *cf_mmap = (void*)hdr + hdr->tp_mac;
 150         *cf = *cf_mmap;
 151         hdr->tp_status = 0;
 152         ctx->current = (ctx->current + 1) % FRAME_NR;
 153         hdr = ctx->ptr + ctx->current*FRAME_SIZE;
 154         return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY;
 155 }
 156
 157
 158 void init_packet_rx(struct in_ctx *ctx)
 159 {
 160         int s;
 161         struct sockaddr_ll my_addr;
 162         struct ifreq ifr;
 163
 164         s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
 165
 166         if (busy_poll_us) {
 167                 CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
 168                                  &busy_poll_us, sizeof(busy_poll_us)));
 169         }
 170
 171         int val = TPACKET_V2;
 172         CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
 173         socklen_t len = sizeof(ctx->hdrlen);
 174         CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
 175
 176         strncpy (ifr.ifr_name, devin, sizeof(ifr.ifr_name));
 177         CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
 178
 179         my_addr.sll_family = AF_PACKET;
 180         my_addr.sll_protocol = htons(ETH_P_ALL);
 181         my_addr.sll_ifindex =  ifr.ifr_ifindex;
 182
 183         CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
 184
 185         struct tpacket_req req = {
 186                 .tp_block_size = BLOCK_SIZE,
 187                 .tp_frame_size = FRAME_SIZE,
 188                 .tp_block_nr   = BLOCK_NR,
 189                 .tp_frame_nr   = FRAME_NR,
 190         };
 191         CHECK(setsockopt(s, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
 192
 193         ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
 194
 195         ctx->s = s;
 196         ctx->in_fn = in_packet_rx;
 197         ctx->current = 0;
 198 }
 199
 200 int out_write(struct out_ctx *ctx, struct can_frame *cf)
 201 {
 202         if (ctx->from_in == NOP)
 203                 return 0;
 204
 205         int ret = write(ctx->s, cf, sizeof(*cf));
 206         if (ret != sizeof(*cf)) {
 207                 perror("write");
 208                 exit(1);
 209         }
 210         return 0;
 211 }
 212
 213 void init_write(struct out_ctx *ctx)
 214 {
 215         int s;
 216         struct sockaddr_can addr;
 217         struct ifreq ifr;
 218
 219         s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
 220
 221         strncpy(ifr.ifr_name, devout, sizeof(ifr.ifr_name));
 222         if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
 223                 perror(devout);
 224                 exit(1);
 225         }
 226
 227         addr.can_family = AF_CAN;
 228         addr.can_ifindex = ifr.ifr_ifindex;
 229
 230         CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
 231
 232         ctx->s = s;
 233         ctx->out_fn = out_write;
 234 }
 235
 236 int out_packet_tx(struct out_ctx *ctx, struct can_frame *cf)
 237 {
 238         volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
 239
 240         if (ctx->from_in == NOP) {
 241                 CHECK(send(ctx->s, NULL, 0, 0));
 242                 return 0;
 243         }
 244
 245         while (hdr->tp_status != TP_STATUS_AVAILABLE) {
 246                 CHECK(send(ctx->s, NULL, 0, 0));
 247         }
 248
 249         //struct sockaddr_ll *addr = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
 250         struct can_frame *cf_mmap = (void*)hdr + TPACKET_HDRLEN  - sizeof(struct sockaddr_ll);
 251         *cf_mmap = *cf;
 252         hdr->tp_len = sizeof(*cf);
 253         hdr->tp_status = TP_STATUS_SEND_REQUEST;
 254         ctx->current = (ctx->current + 1) % FRAME_NR;
 255
 256         if (ctx->from_in == SEND){
 257                 CHECK(send(ctx->s, NULL, 0, 0));
 258                 return 0;
 259         }
 260         return 0;
 261 }
 262
 263
 264 void init_packet_tx(struct out_ctx *ctx)
 265 {
 266         int s;
 267         struct sockaddr_ll my_addr;
 268         struct ifreq ifr;
 269
 270         s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
 271
 272         int val = TPACKET_V2;
 273         CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
 274         socklen_t len = sizeof(ctx->hdrlen);
 275         CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
 276
 277         strncpy (ifr.ifr_name, devout, sizeof(ifr.ifr_name));
 278         CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
 279
 280         my_addr.sll_family = AF_PACKET;
 281         my_addr.sll_protocol = htons(ETH_P_CAN);
 282         my_addr.sll_ifindex =  ifr.ifr_ifindex;
 283
 284         CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
 285
 286         struct tpacket_req req = {
 287                 .tp_block_size = BLOCK_SIZE,
 288                 .tp_frame_size = FRAME_SIZE,
 289                 .tp_block_nr   = BLOCK_NR,
 290                 .tp_frame_nr   = FRAME_NR,
 291         };
 292         CHECK(setsockopt(s, SOL_PACKET, PACKET_TX_RING, (char *)&req, sizeof(req)));
 293
 294         ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
 295
 296         ctx->s = s;
 297         ctx->out_fn = out_packet_tx;
 298         ctx->current = 0;
 299 }
 300
 301 void init_in(struct in_ctx *in)
 302 {
 303         memset(in, 0, sizeof(*in));
 304         switch (in_method) {
 305         case IN_READ:
 306                 init_read(in);
 307                 break;
 308         case IN_MMAP:
 309         case IN_MMAPBUSY:
 310                 init_packet_rx(in);
 311                 break;
 312         default:
 313                 fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
 314                 exit(1);
 315         }
 316 }
 317
 318 int in(struct in_ctx *ctx, struct can_frame *cf)
 319 {
 320         return ctx->in_fn(ctx, cf);
 321 }
 322
 323 void init_out(struct out_ctx *out)
 324 {
 325         memset(out, 0, sizeof(*out));
 326         switch (out_method) {
 327         case WRITE: init_write(out); break;
 328         case OUT_MMAP: init_packet_tx(out); break;
 329         default:
 330                 fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
 331                 exit(1);
 332         }
 333 }
 334
 335 int out(struct out_ctx *ctx, struct can_frame *cf)
 336 {
 337         return ctx->out_fn(ctx, cf);
 338 }
 339
 340 void gw()
 341 {
 342         struct in_ctx    ic;
 343         struct out_ctx   oc;
 344         struct can_frame cf;
 345
 346         init_in(&ic);
 347         init_out(&oc);
 348
 349         VERBOSE("UGW started\n");
 350
 351         while (1) {
 352                 oc.from_in = in(&ic, &cf);
 353                 switch (oc.from_in) {
 354                 case SEND:       stats.send++;  break;
 355                 case STORE_ONLY: stats.store++; break;
 356                 case NOP:                       break;
 357                 }
 358                 out(&oc, &cf);
 359         }
 360 }
 361
 362
 363 int main(int argc, char *argv[])
 364 {
 365         int opt;
 366
 367         while ((opt = getopt(argc, argv, "b:nqr:t:")) != -1) {
 368                 switch (opt) {
 369                 case 'b':
 370                         busy_poll_us = atoi(optarg);
 371                         break;
 372                 case 'n':
 373                         nonblocking = true;
 374                         break;
 375                 case 'q':
 376                         quiet = true;
 377                         break;
 378                 case 'r':
 379                         if (strcmp(optarg, "read") == 0)
 380                                 in_method = IN_READ;
 381                         else if (strcmp(optarg, "mmap") == 0)
 382                                 in_method = IN_MMAP;
 383                         else if (strcmp(optarg, "mmapbusy") == 0)
 384                                 in_method = IN_MMAPBUSY;
 385                         else {
 386                                 fprintf(stderr, "Unsuported RX method: %s\n", optarg);
 387                                 exit(1);
 388                         }
 389                         break;
 390                 case 't':
 391                         if (strcmp(optarg, "write") == 0)
 392                                 out_method = WRITE;
 393                         else if (strcmp(optarg, "mmap") == 0)
 394                                 out_method = OUT_MMAP;
 395                         else {
 396                                 fprintf(stderr, "Unsuported TX method: %s\n", optarg);
 397                                 exit(1);
 398                         }
 399                         break;
 400                 default: /* '?' */
 401                         fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
 402                                 argv[0]);
 403                         exit(EXIT_FAILURE);
 404                 }
 405         }
 406
 407         if (optind < argc)
 408                 devin  = argv[optind];
 409         if (optind+1 < argc)
 410                 devout = argv[optind+1];
 411
 412         signal(SIGINT, sigint);
 413         gw();
 414
 415         return 0;
 416 }