1 /**************************************************************************/
2 /* CAN user space gateway for performance benchmarks */
3 /* Copyright (C) 2013, 2014 Michal Sojka, DCE, FEE, CTU Prague */
5 /**************************************************************************/
8 #include <sys/socket.h>
10 #include <sys/ioctl.h>
18 #include <linux/if_ether.h>
19 #include <linux/if_packet.h>
22 #include <arpa/inet.h>
24 #define FRAME_SIZE 256
25 #define BLOCK_SIZE 4096
27 #define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
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; })
35 char *devout = "can1";
36 enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
37 enum { WRITE } out_method = WRITE;
40 #define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
48 int (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
54 int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
57 int in_read(struct in_ctx *ctx, struct can_frame *cf)
59 int ret = read(ctx->s, cf, sizeof(*cf));
60 if (ret != sizeof(*cf)) {
67 void init_read(struct in_ctx *ctx)
70 struct sockaddr_can addr;
73 s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
75 strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name));
76 if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
81 addr.can_family = AF_CAN;
82 addr.can_ifindex = ifr.ifr_ifindex;
84 CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
90 int in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
92 volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
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));
102 //struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
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);
108 ctx->current = (ctx->current + 1) % FRAME_NR;
113 void init_packet_rx(struct in_ctx *ctx)
116 struct sockaddr_ll my_addr;
119 s = CHECK(socket(PF_PACKET, 1 ? SOCK_RAW : SOCK_DGRAM, htons(ETH_P_ALL)));
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));
126 strncpy (ifr.ifr_name, devin, sizeof(ifr.ifr_name));
127 CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
129 my_addr.sll_family = AF_PACKET;
130 my_addr.sll_protocol = htons(ETH_P_ALL);
131 my_addr.sll_ifindex = ifr.ifr_ifindex;
133 CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
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,
141 CHECK(setsockopt(s, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
143 ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
146 ctx->in_fn = in_packet_rx;
150 int out_write(struct out_ctx *ctx, struct can_frame *cf)
152 int ret = write(ctx->s, cf, sizeof(*cf));
153 if (ret != sizeof(*cf)) {
160 void init_write(struct out_ctx *ctx)
163 struct sockaddr_can addr;
166 s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
168 strncpy(ifr.ifr_name, devout, sizeof(ifr.ifr_name));
169 if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
174 addr.can_family = AF_CAN;
175 addr.can_ifindex = ifr.ifr_ifindex;
177 CHECK(bind(s, (struct sockaddr *)&addr, sizeof(addr)));
180 ctx->out_fn = out_write;
183 void init_in(struct in_ctx *in)
194 fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
199 int in(struct in_ctx *ctx, struct can_frame *cf)
201 return ctx->in_fn(ctx, cf);
204 void init_out(struct out_ctx *out)
206 switch (out_method) {
207 case WRITE: init_write(out); break;
209 fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
214 int out(struct out_ctx *ctx, struct can_frame *cf)
216 return ctx->out_fn(ctx, cf);
228 VERBOSE("UGW started\n");
232 oc.from_in = ic.for_out;
238 int main(int argc, char *argv[])
242 while ((opt = getopt(argc, argv, "qr:")) != -1) {
248 if (strcmp(optarg, "read") == 0)
250 else if (strcmp(optarg, "mmap") == 0)
252 else if (strcmp(optarg, "mmapbusy") == 0)
253 in_method = IN_MMAPBUSY;
255 fprintf(stderr, "Unsuported RX method: %s\n", optarg);
260 fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
267 devin = argv[optind];
269 devout = argv[optind+1];