X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/89d198f9565e5c693252280ed8154a8cb3d131b5..052fa871a1c16da85562fba8b256287b404d7bab:/ugw/ugw.c

diff --git a/ugw/ugw.c b/ugw/ugw.c
index 64d00b0..e2c04ef 100644
--- a/ugw/ugw.c
+++ b/ugw/ugw.c
@@ -20,8 +20,15 @@
 #include <poll.h>
 #include <sys/mman.h>
 #include <arpa/inet.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <signal.h>
 
-#define FRAME_SIZE 256
+#ifndef SO_BUSY_POLL
+#define SO_BUSY_POLL 46
+#endif
+
+#define FRAME_SIZE 128
 #define BLOCK_SIZE 4096
 #define BLOCK_NR 2
 #define FRAME_NR (BLOCK_NR*(BLOCK_SIZE/FRAME_SIZE))
@@ -36,10 +43,13 @@ char *devout = "can1";
 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,
 };
 
 
@@ -62,9 +72,23 @@ struct out_ctx {
 	int (*out_fn)(struct out_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);
@@ -80,6 +104,16 @@ void init_read(struct in_ctx *ctx)
 
 	s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW));
 
+	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);
@@ -110,7 +144,6 @@ enum in2out in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
 	//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;
@@ -127,6 +160,11 @@ void init_packet_rx(struct in_ctx *ctx)
 
 	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);
@@ -158,6 +196,9 @@ void init_packet_rx(struct in_ctx *ctx)
 
 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");
@@ -192,14 +233,14 @@ void init_write(struct out_ctx *ctx)
 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));
+		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;
@@ -208,7 +249,7 @@ int out_packet_tx(struct out_ctx *ctx, struct can_frame *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;
@@ -304,6 +345,11 @@ void gw()
 
 	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);
 	}
 }
@@ -313,8 +359,14 @@ int main(int argc, char *argv[])
 {
 	int opt;
 
-	while ((opt = getopt(argc, argv, "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;
@@ -352,6 +404,7 @@ int main(int argc, char *argv[])
 	if (optind+1 < argc)
 		devout = argv[optind+1];
 
+	signal(SIGINT, sigint);
 	gw();
 
 	return 0;