X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/aa3c50e2235871a58e803fbe4ea70fb0172fe1ee..08ba26a2b3e7aa9c039f564fc2124999d63cb587:/ugw/ugw.c

diff --git a/ugw/ugw.c b/ugw/ugw.c
index 23d438c..bb654cf 100644
--- a/ugw/ugw.c
+++ b/ugw/ugw.c
@@ -34,34 +34,42 @@
 char *devin = "can0";
 char *devout = "can1";
 enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
-enum { WRITE } 	       out_method = WRITE;
+enum { WRITE, OUT_MMAP } out_method = WRITE;
 bool quiet = false;
 
+enum in2out {
+	STORE_ONLY,
+	SEND,
+};
+
+
 #define VERBOSE(format, ...) do { if (!quiet) fprintf(stderr, format, ##__VA_ARGS__); } while (0)
 
 struct in_ctx {
 	int s;
-	int for_out;
 	void *ptr;
 	int hdrlen;
 	int current;
-	int (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
+	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)
+enum in2out in_read(struct in_ctx *ctx, struct can_frame *cf)
 {
 	int ret = read(ctx->s, cf, sizeof(*cf));
 	if (ret != sizeof(*cf)) {
 		perror("read");
 		exit(1);
 	}
-	return 0;
+	return SEND;
 }
 
 void init_read(struct in_ctx *ctx)
@@ -87,7 +95,7 @@ void init_read(struct in_ctx *ctx)
 	ctx->in_fn = in_read;
 }
 
-int in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
+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;
@@ -106,7 +114,8 @@ int in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
 	*cf = *cf_mmap;
 	hdr->tp_status = 0;
 	ctx->current = (ctx->current + 1) % FRAME_NR;
-	return 0;
+	hdr = ctx->ptr + ctx->current*FRAME_SIZE;
+	return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY;
 }
 
 
@@ -116,7 +125,7 @@ void init_packet_rx(struct in_ctx *ctx)
 	struct sockaddr_ll my_addr;
 	struct ifreq ifr;
 
-	s = CHECK(socket(PF_PACKET, 1 ? SOCK_RAW : SOCK_DGRAM, htons(ETH_P_ALL)));
+	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)));
@@ -180,8 +189,74 @@ void init_write(struct out_ctx *ctx)
 	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;
+
+	while (hdr->tp_status != TP_STATUS_AVAILABLE) {
+		printf("TX status: %#x\n", hdr->tp_status);
+		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));
+		printf("send:%d\n", ret);
+		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_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_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 IN_READ:
 		init_read(in);
@@ -203,8 +278,10 @@ int in(struct in_ctx *ctx, struct can_frame *cf)
 
 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);
@@ -228,8 +305,7 @@ void gw()
 	VERBOSE("UGW started\n");
 
 	while (1) {
-		in(&ic, &cf);
-		oc.from_in = ic.for_out;
+		oc.from_in = in(&ic, &cf);
 		out(&oc, &cf);
 	}
 }
@@ -239,7 +315,7 @@ int main(int argc, char *argv[])
 {
 	int opt;
 
-	while ((opt = getopt(argc, argv, "qr:")) != -1) {
+	while ((opt = getopt(argc, argv, "qr:t:")) != -1) {
 		switch (opt) {
 		case 'q':
 			quiet = true;
@@ -256,6 +332,16 @@ int main(int argc, char *argv[])
 				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]);