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