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