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_method = WRITE;
  38 bool quiet = false;
  39
  40 #define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
  41
  42 struct in_ctx {
  43         int s;
  44         int for_out;
  45         void *ptr;
  46         int hdrlen;
  47         int current;
  48         int (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
  49 };
  50
  51 struct out_ctx {
  52         int s;
  53         int from_in;
  54         int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
  55 };
  56
  57 int in_read(struct in_ctx *ctx, struct can_frame *cf)
  58 {
  59         int ret = read(ctx->s, cf, sizeof(*cf));
  60         if (ret != sizeof(*cf)) {
  61                 perror("read");
  62                 exit(1);
  63         }
  64         return 0;
  65 }
  66
  67 void init_read(struct in_ctx *ctx)
  68 {
  69         int s;
  70         struct sockaddr_can addr;
  71         struct ifreq ifr;
  72
  73         s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
  74
  75         strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name));
  76         if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
  77                 perror(devin);
  78                 exit(1);
  79         }
  80
  81         addr.can_family = AF_CAN;
  82         addr.can_ifindex = ifr.ifr_ifindex;
  83
  84         CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
  85
  86         ctx->s = s;
  87         ctx->in_fn = in_read;
  88 }
  89
  90 int in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
  91 {
  92         volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
  93         int ret = -1;
  94
  95         while (hdr->tp_status == TP_STATUS_KERNEL) {
  96                 if (in_method != IN_MMAPBUSY) {
  97                         struct pollfd pfd = {.fd = ctx->s, .revents = 0,
  98                                              .events = POLLIN|POLLRDNORM|POLLERR };
  99                         ret = CHECK(poll(&pfd, 1, -1));
 100                 }
 101         }
 102         //struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
 103         (void)ret;
 104         struct can_frame *cf_mmap = (void*)hdr + hdr->tp_mac;
 105         //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);
 106         *cf = *cf_mmap;
 107         hdr->tp_status = 0;
 108         ctx->current = (ctx->current + 1) % FRAME_NR;
 109         return 0;
 110 }
 111
 112
 113 void init_packet_rx(struct in_ctx *ctx)
 114 {
 115         int s;
 116         struct sockaddr_ll my_addr;
 117         struct ifreq ifr;
 118
 119         s = CHECK(socket(PF_PACKET, 1 ? SOCK_RAW : SOCK_DGRAM, htons(ETH_P_ALL)));
 120
 121         int val = TPACKET_V2;
 122         CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
 123         socklen_t len = sizeof(ctx->hdrlen);
 124         CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
 125
 126         strncpy (ifr.ifr_name, devin, sizeof(ifr.ifr_name));
 127         CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
 128
 129         my_addr.sll_family = AF_PACKET;
 130         my_addr.sll_protocol = htons(ETH_P_ALL);
 131         my_addr.sll_ifindex =  ifr.ifr_ifindex;
 132
 133         CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
 134
 135         struct tpacket_req req = {
 136                 .tp_block_size = BLOCK_SIZE,
 137                 .tp_frame_size = FRAME_SIZE,
 138                 .tp_block_nr   = BLOCK_NR,
 139                 .tp_frame_nr   = FRAME_NR,
 140         };
 141         CHECK(setsockopt(s, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
 142
 143         ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
 144
 145         ctx->s = s;
 146         ctx->in_fn = in_packet_rx;
 147         ctx->current = 0;
 148 }
 149
 150 int out_write(struct out_ctx *ctx, struct can_frame *cf)
 151 {
 152         int ret = write(ctx->s, cf, sizeof(*cf));
 153         if (ret != sizeof(*cf)) {
 154                 perror("write");
 155                 exit(1);
 156         }
 157         return 0;
 158 }
 159
 160 void init_write(struct out_ctx *ctx)
 161 {
 162         int s;
 163         struct sockaddr_can addr;
 164         struct ifreq ifr;
 165
 166         s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
 167
 168         strncpy(ifr.ifr_name, devout, sizeof(ifr.ifr_name));
 169         if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
 170                 perror(devout);
 171                 exit(1);
 172         }
 173
 174         addr.can_family = AF_CAN;
 175         addr.can_ifindex = ifr.ifr_ifindex;
 176
 177         CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
 178
 179         ctx->s = s;
 180         ctx->out_fn = out_write;
 181 }
 182
 183 void init_in(struct in_ctx *in)
 184 {
 185         switch (in_method) {
 186         case IN_READ:
 187                 init_read(in);
 188                 break;
 189         case IN_MMAP:
 190         case IN_MMAPBUSY:
 191                 init_packet_rx(in);
 192                 break;
 193         default:
 194                 fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
 195                 exit(1);
 196         }
 197 }
 198
 199 int in(struct in_ctx *ctx, struct can_frame *cf)
 200 {
 201         return ctx->in_fn(ctx, cf);
 202 }
 203
 204 void init_out(struct out_ctx *out)
 205 {
 206         switch (out_method) {
 207         case WRITE: init_write(out); break;
 208         default:
 209                 fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
 210                 exit(1);
 211         }
 212 }
 213
 214 int out(struct out_ctx *ctx, struct can_frame *cf)
 215 {
 216         return ctx->out_fn(ctx, cf);
 217 }
 218
 219 void gw()
 220 {
 221         struct in_ctx    ic;
 222         struct out_ctx   oc;
 223         struct can_frame cf;
 224
 225         init_in(&ic);
 226         init_out(&oc);
 227
 228         VERBOSE("UGW started\n");
 229
 230         while (1) {
 231                 in(&ic, &cf);
 232                 oc.from_in = ic.for_out;
 233                 out(&oc, &cf);
 234         }
 235 }
 236
 237
 238 int main(int argc, char *argv[])
 239 {
 240         int opt;
 241
 242         while ((opt = getopt(argc, argv, "qr:")) != -1) {
 243                 switch (opt) {
 244                 case 'q':
 245                         quiet = true;
 246                         break;
 247                 case 'r':
 248                         if (strcmp(optarg, "read") == 0)
 249                                 in_method = IN_READ;
 250                         else if (strcmp(optarg, "mmap") == 0)
 251                                 in_method = IN_MMAP;
 252                         else if (strcmp(optarg, "mmapbusy") == 0)
 253                                 in_method = IN_MMAPBUSY;
 254                         else {
 255                                 fprintf(stderr, "Unsuported RX method: %s\n", optarg);
 256                                 exit(1);
 257                         }
 258                         break;
 259                 default: /* '?' */
 260                         fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
 261                                 argv[0]);
 262                         exit(EXIT_FAILURE);
 263                 }
 264         }
 265
 266         if (optind < argc)
 267                 devin  = argv[optind];
 268         if (optind+1 < argc)
 269                 devout = argv[optind+1];
 270
 271         gw();
 272
 273         return 0;
 274 }