Add PF_PACKET + mmap() support for RX in ugw
[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_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 }