Add mmap()-based TX method - it doesn't work (yet?)
[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 #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                 printf("TX status: %#x\n", hdr->tp_status);
199                 struct pollfd pfd = {.fd = ctx->s, .revents = 0,
200                                      .events = POLLIN|POLLRDNORM|POLLERR };
201                 ret = CHECK(poll(&pfd, 1, -1));
202         }
203         (void)ret;
204         //struct sockaddr_ll *addr = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
205         struct can_frame *cf_mmap = (void*)hdr + TPACKET_HDRLEN  - sizeof(struct sockaddr_ll);
206         *cf_mmap = *cf;
207         hdr->tp_len = sizeof(*cf);
208         hdr->tp_status = TP_STATUS_SEND_REQUEST;
209         ctx->current = (ctx->current + 1) % FRAME_NR;
210
211         if (ctx->from_in == SEND){
212                 ret = CHECK(send(ctx->s, NULL, 0, 0));
213                 printf("send:%d\n", ret);
214                 return 0;
215         }
216         return 0;
217 }
218
219
220 void init_packet_tx(struct out_ctx *ctx)
221 {
222         int s;
223         struct sockaddr_ll my_addr;
224         struct ifreq ifr;
225
226         s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
227
228         int val = TPACKET_V2;
229         CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
230         socklen_t len = sizeof(ctx->hdrlen);
231         CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
232
233         strncpy (ifr.ifr_name, devout, sizeof(ifr.ifr_name));
234         CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
235
236         my_addr.sll_family = AF_PACKET;
237         my_addr.sll_protocol = htons(ETH_P_ALL);
238         my_addr.sll_ifindex =  ifr.ifr_ifindex;
239
240         CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
241
242         struct tpacket_req req = {
243                 .tp_block_size = BLOCK_SIZE,
244                 .tp_frame_size = FRAME_SIZE,
245                 .tp_block_nr   = BLOCK_NR,
246                 .tp_frame_nr   = FRAME_NR,
247         };
248         CHECK(setsockopt(s, SOL_PACKET, PACKET_TX_RING, (char *)&req, sizeof(req)));
249
250         ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
251
252         ctx->s = s;
253         ctx->out_fn = out_packet_tx;
254         ctx->current = 0;
255 }
256
257 void init_in(struct in_ctx *in)
258 {
259         memset(in, 0, sizeof(*in));
260         switch (in_method) {
261         case IN_READ:
262                 init_read(in);
263                 break;
264         case IN_MMAP:
265         case IN_MMAPBUSY:
266                 init_packet_rx(in);
267                 break;
268         default:
269                 fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
270                 exit(1);
271         }
272 }
273
274 int in(struct in_ctx *ctx, struct can_frame *cf)
275 {
276         return ctx->in_fn(ctx, cf);
277 }
278
279 void init_out(struct out_ctx *out)
280 {
281         memset(out, 0, sizeof(*out));
282         switch (out_method) {
283         case WRITE: init_write(out); break;
284         case OUT_MMAP: init_packet_tx(out); break;
285         default:
286                 fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
287                 exit(1);
288         }
289 }
290
291 int out(struct out_ctx *ctx, struct can_frame *cf)
292 {
293         return ctx->out_fn(ctx, cf);
294 }
295
296 void gw()
297 {
298         struct in_ctx    ic;
299         struct out_ctx   oc;
300         struct can_frame cf;
301
302         init_in(&ic);
303         init_out(&oc);
304
305         VERBOSE("UGW started\n");
306
307         while (1) {
308                 oc.from_in = in(&ic, &cf);
309                 out(&oc, &cf);
310         }
311 }
312
313
314 int main(int argc, char *argv[])
315 {
316         int opt;
317
318         while ((opt = getopt(argc, argv, "qr:t:")) != -1) {
319                 switch (opt) {
320                 case 'q':
321                         quiet = true;
322                         break;
323                 case 'r':
324                         if (strcmp(optarg, "read") == 0)
325                                 in_method = IN_READ;
326                         else if (strcmp(optarg, "mmap") == 0)
327                                 in_method = IN_MMAP;
328                         else if (strcmp(optarg, "mmapbusy") == 0)
329                                 in_method = IN_MMAPBUSY;
330                         else {
331                                 fprintf(stderr, "Unsuported RX method: %s\n", optarg);
332                                 exit(1);
333                         }
334                         break;
335                 case 't':
336                         if (strcmp(optarg, "write") == 0)
337                                 out_method = WRITE;
338                         else if (strcmp(optarg, "mmap") == 0)
339                                 out_method = OUT_MMAP;
340                         else {
341                                 fprintf(stderr, "Unsuported TX method: %s\n", optarg);
342                                 exit(1);
343                         }
344                         break;
345                 default: /* '?' */
346                         fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
347                                 argv[0]);
348                         exit(EXIT_FAILURE);
349                 }
350         }
351
352         if (optind < argc)
353                 devin  = argv[optind];
354         if (optind+1 < argc)
355                 devout = argv[optind+1];
356
357         gw();
358
359         return 0;
360 }