Added ftrace config (commented oud)
[can-benchmark.git] / 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 }