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