#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 FRAME_SIZE 128
#define BLOCK_SIZE 4096
#define BLOCK_NR 2
#define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
+#define MMSG_MAX 64
+
#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; })
char *devin = "can0";
char *devout = "can1";
enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
-enum { WRITE, OUT_MMAP } out_method = WRITE;
+enum { WRITE, OUT_MMAP, OUT_SENDMMSG } out_method = WRITE;
bool quiet = false;
int busy_poll_us = 0;
+bool nonblocking = false;
enum in2out {
STORE_ONLY,
SEND,
+ NOP,
};
struct in_ctx {
int s;
+ enum in2out (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
+ /* mmap */
void *ptr;
int hdrlen;
int current;
- enum in2out (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
+ /* mmsg */
+ int received;
+ struct mmsghdr msgs[MMSG_MAX];
+ struct iovec iovecs[MMSG_MAX];
+ char bufs[MMSG_MAX][sizeof(struct can_frame)];
};
struct out_ctx {
int s;
enum in2out from_in;
+ int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
+ /* mmap */
void *ptr;
int hdrlen;
int current;
- int (*out_fn)(struct out_ctx *ctx, struct can_frame *cf);
+ /* mmsg */
+ int stored;
+ struct mmsghdr msgs[MMSG_MAX];
+ struct iovec iovecs[MMSG_MAX];
+ char bufs[MMSG_MAX][sizeof(struct can_frame)];
};
+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 SEND;
+ return STORE_ONLY;
}
void init_read(struct in_ctx *ctx)
s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
+ int rcvbuf = 25000; /* Limit rcvbuf to not have so big queueing latencies */
+ CHECK(setsockopt(s, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof(rcvbuf)));
+
+ 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)));
ctx->in_fn = in_read;
}
+enum in2out in_recvmmsg(struct in_ctx *ctx, struct can_frame *cf)
+{
+ int ret;
+
+ if (ctx->current >= ctx->received) {
+ ret = CHECK(recvmmsg(ctx->s, ctx->msgs, MMSG_MAX, MSG_WAITFORONE, NULL));
+ if (nonblocking && ret == -1 && errno == EAGAIN)
+ return NOP;
+ ctx->received = ret;
+ ctx->current = 0;
+ }
+
+ memcpy(cf, ctx->bufs[ctx->current], sizeof(struct can_frame));
+ ctx->current++;
+
+ return (ctx->current < ctx->received) ? STORE_ONLY : SEND;
+}
+
+void init_recvmmg(struct in_ctx *ctx)
+{
+ int i;
+
+ init_read(ctx);
+ ctx->in_fn = in_recvmmsg;
+ ctx->current = 0;
+ memset(ctx->msgs, 0, sizeof(ctx->msgs));
+ memset(ctx->iovecs, 0, sizeof(ctx->iovecs));
+ for (i = 0; i < MMSG_MAX; i++) {
+ ctx->iovecs[i].iov_base = ctx->bufs[i];
+ ctx->iovecs[i].iov_len = sizeof(struct can_frame);
+ ctx->msgs[i].msg_hdr.msg_iov = &ctx->iovecs[i];
+ ctx->msgs[i].msg_hdr.msg_iovlen = 1;
+ }
+}
+
enum in2out in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
{
volatile struct tpacket2_hdr *hdr = ctx->ptr + ctx->current*FRAME_SIZE;
//struct sockaddr_ll *addr = (void*)hdr + TPACKET_ALIGN(ctx->hdrlen);
(void)ret;
struct can_frame *cf_mmap = (void*)hdr + hdr->tp_mac;
- //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);
*cf = *cf_mmap;
hdr->tp_status = 0;
ctx->current = (ctx->current + 1) % FRAME_NR;
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");
ctx->out_fn = out_write;
}
+int out_sendmmsg(struct out_ctx *ctx, struct can_frame *cf)
+{
+ if (ctx->from_in == NOP)
+ return 0;
+
+ memcpy(ctx->bufs[ctx->current++], cf, sizeof(*cf));
+
+ if (ctx->from_in == SEND ||
+ ctx->current >= MMSG_MAX) {
+ int ret, i;
+ for (i = 0; i < ctx->current; i += ret)
+ ret = CHECK(sendmmsg(ctx->s, &ctx->msgs[i], ctx->current - i, 0));
+ ctx->current = 0;
+ }
+ return 0;
+}
+
+void init_sendmmsg(struct out_ctx *ctx)
+{
+ int i;
+
+ init_write(ctx);
+ ctx->out_fn = out_sendmmsg;
+ ctx->current = 0;
+ ctx->stored = 0;
+
+ memset(ctx->msgs, 0, sizeof(ctx->msgs));
+ memset(ctx->iovecs, 0, sizeof(ctx->iovecs));
+ for (i = 0; i < MMSG_MAX; i++) {
+ ctx->iovecs[i].iov_base = &ctx->bufs[i];
+ ctx->iovecs[i].iov_len = sizeof(struct can_frame);
+ ctx->msgs[i].msg_hdr.msg_iov = &ctx->iovecs[i];
+ ctx->msgs[i].msg_hdr.msg_iovlen = 1;
+ }
+}
+
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) {
+ CHECK(send(ctx->s, NULL, 0, 0));
+ 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));
+ CHECK(send(ctx->s, NULL, 0, 0));
}
- (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;
ctx->current = (ctx->current + 1) % FRAME_NR;
if (ctx->from_in == SEND){
- ret = CHECK(send(ctx->s, NULL, 0, 0));
+ CHECK(send(ctx->s, NULL, 0, 0));
return 0;
}
return 0;
case IN_READ:
init_read(in);
break;
+ case IN_RECVMMSG:
+ init_recvmmg(in);
+ break;
case IN_MMAP:
case IN_MMAPBUSY:
init_packet_rx(in);
memset(out, 0, sizeof(*out));
switch (out_method) {
case WRITE: init_write(out); break;
+ case OUT_SENDMMSG: init_sendmmsg(out); break;
case OUT_MMAP: init_packet_tx(out); break;
default:
fprintf(stderr, "Unknown \"out method\" %d\n", out_method);
while (1) {
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, "b:qr:t:")) != -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;
in_method = IN_MMAP;
else if (strcmp(optarg, "mmapbusy") == 0)
in_method = IN_MMAPBUSY;
+ else if (strcmp(optarg, "mmsg") == 0)
+ in_method = IN_RECVMMSG;
else {
fprintf(stderr, "Unsuported RX method: %s\n", optarg);
exit(1);
out_method = WRITE;
else if (strcmp(optarg, "mmap") == 0)
out_method = OUT_MMAP;
+ else if (strcmp(optarg, "mmsg") == 0)
+ out_method = OUT_SENDMMSG;
else {
fprintf(stderr, "Unsuported TX method: %s\n", optarg);
exit(1);
if (optind+1 < argc)
devout = argv[optind+1];
+ signal(SIGINT, sigint);
gw();
return 0;