X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/aa3c50e2235871a58e803fbe4ea70fb0172fe1ee..052fa871a1c16da85562fba8b256287b404d7bab:/ugw/ugw.c diff --git a/ugw/ugw.c b/ugw/ugw.c index 23d438c..e2c04ef 100644 --- a/ugw/ugw.c +++ b/ugw/ugw.c @@ -20,8 +20,15 @@ #include #include #include +#include +#include +#include -#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)) @@ -34,34 +41,59 @@ 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; +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; 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) +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) @@ -72,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); @@ -87,7 +129,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; @@ -102,11 +144,11 @@ int 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; - return 0; + hdr = ctx->ptr + ctx->current*FRAME_SIZE; + return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY; } @@ -116,7 +158,12 @@ 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))); + + 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))); @@ -149,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"); @@ -180,8 +230,72 @@ 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; + + if (ctx->from_in == NOP) + return 0; + + while (hdr->tp_status != TP_STATUS_AVAILABLE) { + CHECK(send(ctx->s, NULL, 0, 0)); + } + + //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){ + 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 IN_READ: init_read(in); @@ -203,8 +317,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 +344,12 @@ void gw() 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); } } @@ -239,8 +359,14 @@ int main(int argc, char *argv[]) { int opt; - while ((opt = getopt(argc, argv, "qr:")) != -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; @@ -256,6 +382,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]); @@ -268,6 +404,7 @@ int main(int argc, char *argv[]) if (optind+1 < argc) devout = argv[optind+1]; + signal(SIGINT, sigint); gw(); return 0;