#include <stdio.h>
#include <time.h>
#include <stdbool.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <poll.h>
+#include <sys/mman.h>
+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <signal.h>
+
+#ifndef SO_BUSY_POLL
+#define SO_BUSY_POLL 46
+#endif
+
+#define FRAME_SIZE 256
+#define BLOCK_SIZE 4096
+#define BLOCK_NR 2
+#define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
#define STRINGIFY(val) #val
#define TOSTRING(val) STRINGIFY(val)
#define CHECK(cmd) ({ int ret = (cmd); if (ret == -1) { perror(#cmd " line " TOSTRING(__LINE__)); exit(1); }; ret; })
+#define CHECKPTR(cmd) ({ void *ptr = (cmd); if (ptr == (void*)-1) { perror(#cmd " line " TOSTRING(__LINE__)); exit(1); }; ptr; })
char *devin = "can0";
char *devout = "can1";
-enum { READ, RECVMMSG } in_method = READ;
-enum { WRITE } out_method = WRITE;
+enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
+enum { WRITE, OUT_MMAP } out_method = WRITE;
bool quiet = false;
+int busy_poll_us = 0;
+bool nonblocking = false;
+
+enum in2out {
+ STORE_ONLY,
+ SEND,
+ NOP,
+};
+
#define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
struct in_ctx {
int s;
- int for_out;
- int (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
+ void *ptr;
+ int hdrlen;
+ int current;
+ enum in2out (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
};
struct out_ctx {
int s;
- int from_in;
+ enum in2out from_in;
+ void *ptr;
+ int hdrlen;
+ int current;
int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
};
-int in_read(struct in_ctx *ctx, struct can_frame *cf)
+struct stats {
+ int store;
+ int send;
+} stats;
+
+void sigint(int v)
+{
+ printf("store:%d\nsend:%d\ntotal:%d\n",
+ stats.store, stats.send, stats.store + stats.send);
+ exit(0);
+}
+
+enum in2out in_read(struct in_ctx *ctx, struct can_frame *cf)
{
int ret = read(ctx->s, cf, sizeof(*cf));
+ if (nonblocking && ret == -1 && errno == EAGAIN)
+ return NOP;
if (ret != sizeof(*cf)) {
perror("read");
exit(1);
}
- return 0;
+ return SEND;
}
void init_read(struct in_ctx *ctx)
s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
- strcpy(ifr.ifr_name, devin);
+ if (nonblocking) {
+ int flags = CHECK(fcntl(s, F_GETFL, 0));
+ CHECK(fcntl(s, F_SETFL, flags | O_NONBLOCK));
+ }
+
+ if (busy_poll_us) {
+ CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
+ &busy_poll_us, sizeof(busy_poll_us)));
+ }
+
+ strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name));
if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
perror(devin);
exit(1);
ctx->in_fn = in_read;
}
+enum in2out in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
+{
+ volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
+ int ret = -1;
+
+ while (hdr->tp_status == TP_STATUS_KERNEL) {
+ if (in_method != IN_MMAPBUSY) {
+ struct pollfd pfd = {.fd = ctx->s, .revents = 0,
+ .events = POLLIN|POLLRDNORM|POLLERR };
+ ret = CHECK(poll(&pfd, 1, -1));
+ }
+ }
+ //struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
+ (void)ret;
+ struct can_frame *cf_mmap = (void*)hdr + hdr->tp_mac;
+ *cf = *cf_mmap;
+ hdr->tp_status = 0;
+ ctx->current = (ctx->current + 1) % FRAME_NR;
+ hdr = ctx->ptr + ctx->current*FRAME_SIZE;
+ return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY;
+}
+
+
+void init_packet_rx(struct in_ctx *ctx)
+{
+ int s;
+ struct sockaddr_ll my_addr;
+ struct ifreq ifr;
+
+ s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
+
+ if (busy_poll_us) {
+ CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
+ &busy_poll_us, sizeof(busy_poll_us)));
+ }
+
+ int val = TPACKET_V2;
+ CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
+ socklen_t len = sizeof(ctx->hdrlen);
+ CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
+
+ strncpy (ifr.ifr_name, devin, sizeof(ifr.ifr_name));
+ CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
+
+ my_addr.sll_family = AF_PACKET;
+ my_addr.sll_protocol = htons(ETH_P_ALL);
+ my_addr.sll_ifindex = ifr.ifr_ifindex;
+
+ CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
+
+ struct tpacket_req req = {
+ .tp_block_size = BLOCK_SIZE,
+ .tp_frame_size = FRAME_SIZE,
+ .tp_block_nr = BLOCK_NR,
+ .tp_frame_nr = FRAME_NR,
+ };
+ CHECK(setsockopt(s, SOL_PACKET, PACKET_RX_RING, (char *)&req, sizeof(req)));
+
+ ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
+
+ ctx->s = s;
+ ctx->in_fn = in_packet_rx;
+ ctx->current = 0;
+}
+
int out_write(struct out_ctx *ctx, struct can_frame *cf)
{
+ if (ctx->from_in == NOP)
+ return 0;
+
int ret = write(ctx->s, cf, sizeof(*cf));
if (ret != sizeof(*cf)) {
perror("write");
s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
- strcpy(ifr.ifr_name, devout);
+ strncpy(ifr.ifr_name, devout, sizeof(ifr.ifr_name));
if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) {
perror(devout);
exit(1);
ctx->out_fn = out_write;
}
+int out_packet_tx(struct out_ctx *ctx, struct can_frame *cf)
+{
+ volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
+ int ret = -1;
+
+ if (ctx->from_in == NOP)
+ return 0;
+
+
+ while (hdr->tp_status != TP_STATUS_AVAILABLE) {
+ struct pollfd pfd = {.fd = ctx->s, .revents = 0,
+ .events = POLLIN|POLLRDNORM|POLLERR };
+ ret = CHECK(poll(&pfd, 1, -1));
+ }
+ (void)ret;
+ //struct sockaddr_ll *addr = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
+ struct can_frame *cf_mmap = (void*)hdr + TPACKET_HDRLEN - sizeof(struct sockaddr_ll);
+ *cf_mmap = *cf;
+ hdr->tp_len = sizeof(*cf);
+ hdr->tp_status = TP_STATUS_SEND_REQUEST;
+ ctx->current = (ctx->current + 1) % FRAME_NR;
+
+ if (ctx->from_in == SEND){
+ ret = CHECK(send(ctx->s, NULL, 0, 0));
+ return 0;
+ }
+ return 0;
+}
+
+
+void init_packet_tx(struct out_ctx *ctx)
+{
+ int s;
+ struct sockaddr_ll my_addr;
+ struct ifreq ifr;
+
+ s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
+
+ int val = TPACKET_V2;
+ CHECK(setsockopt(s, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)));
+ socklen_t len = sizeof(ctx->hdrlen);
+ CHECK(getsockopt(s, SOL_PACKET, PACKET_HDRLEN, &ctx->hdrlen, &len));
+
+ strncpy (ifr.ifr_name, devout, sizeof(ifr.ifr_name));
+ CHECK(ioctl(s, SIOCGIFINDEX, &ifr));
+
+ my_addr.sll_family = AF_PACKET;
+ my_addr.sll_protocol = htons(ETH_P_CAN);
+ my_addr.sll_ifindex = ifr.ifr_ifindex;
+
+ CHECK(bind(s, (struct sockaddr *)&my_addr, sizeof(struct sockaddr_ll)));
+
+ struct tpacket_req req = {
+ .tp_block_size = BLOCK_SIZE,
+ .tp_frame_size = FRAME_SIZE,
+ .tp_block_nr = BLOCK_NR,
+ .tp_frame_nr = FRAME_NR,
+ };
+ CHECK(setsockopt(s, SOL_PACKET, PACKET_TX_RING, (char *)&req, sizeof(req)));
+
+ ctx->ptr = (char*)CHECKPTR(mmap(0, BLOCK_SIZE*BLOCK_NR, PROT_READ|PROT_WRITE, MAP_SHARED, s, 0));
+
+ ctx->s = s;
+ ctx->out_fn = out_packet_tx;
+ ctx->current = 0;
+}
+
void init_in(struct in_ctx *in)
{
+ memset(in, 0, sizeof(*in));
switch (in_method) {
- case READ: init_read(in); break;
+ case IN_READ:
+ init_read(in);
+ break;
+ case IN_MMAP:
+ case IN_MMAPBUSY:
+ init_packet_rx(in);
+ break;
default:
fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
exit(1);
void init_out(struct out_ctx *out)
{
+ memset(out, 0, sizeof(*out));
switch (out_method) {
case WRITE: init_write(out); break;
+ case OUT_MMAP: init_packet_tx(out); break;
default:
fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
exit(1);
init_in(&ic);
init_out(&oc);
- VERBOSE("UGW started");
+ VERBOSE("UGW started\n");
while (1) {
- in(&ic, &cf);
- oc.from_in = ic.for_out;
+ oc.from_in = in(&ic, &cf);
+ switch (oc.from_in) {
+ case SEND: stats.send++; break;
+ case STORE_ONLY: stats.store++; break;
+ case NOP: break;
+ }
out(&oc, &cf);
}
}
{
int opt;
- while ((opt = getopt(argc, argv, "q")) != -1) {
+ while ((opt = getopt(argc, argv, "b:nqr:t:")) != -1) {
switch (opt) {
+ case 'b':
+ busy_poll_us = atoi(optarg);
+ break;
+ case 'n':
+ nonblocking = true;
+ break;
case 'q':
quiet = true;
break;
+ case 'r':
+ if (strcmp(optarg, "read") == 0)
+ in_method = IN_READ;
+ else if (strcmp(optarg, "mmap") == 0)
+ in_method = IN_MMAP;
+ else if (strcmp(optarg, "mmapbusy") == 0)
+ in_method = IN_MMAPBUSY;
+ else {
+ fprintf(stderr, "Unsuported RX method: %s\n", optarg);
+ exit(1);
+ }
+ break;
+ case 't':
+ if (strcmp(optarg, "write") == 0)
+ out_method = WRITE;
+ else if (strcmp(optarg, "mmap") == 0)
+ out_method = OUT_MMAP;
+ else {
+ fprintf(stderr, "Unsuported TX method: %s\n", optarg);
+ exit(1);
+ }
+ break;
default: /* '?' */
fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
argv[0]);
if (optind+1 < argc)
devout = argv[optind+1];
+ signal(SIGINT, sigint);
gw();
return 0;