X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/36a8c1dbdb6af0d11d233ba49c92d3733b3eac17..08ba26a2b3e7aa9c039f564fc2124999d63cb587:/ugw/ugw.c diff --git a/ugw/ugw.c b/ugw/ugw.c index a3395c3..bb654cf 100644 --- a/ugw/ugw.c +++ b/ugw/ugw.c @@ -15,39 +15,61 @@ #include #include #include +#include +#include +#include +#include +#include + +#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; +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; - 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) +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) @@ -58,7 +80,7 @@ void init_read(struct in_ctx *ctx) s = CHECK(socket(PF_CAN, SOCK_RAW, CAN_RAW)); - strcpy(ifr.ifr_name, devin); + strncpy(ifr.ifr_name, devin, sizeof(ifr.ifr_name)); if (-1 == ioctl(s, SIOCGIFINDEX, &ifr)) { perror(devin); exit(1); @@ -73,6 +95,67 @@ void init_read(struct in_ctx *ctx) 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; + //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; + 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))); + + 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) { int ret = write(ctx->s, cf, sizeof(*cf)); @@ -91,7 +174,7 @@ void init_write(struct out_ctx *ctx) 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); @@ -106,10 +189,82 @@ 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 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); @@ -123,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); @@ -145,11 +302,10 @@ void gw() 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); out(&oc, &cf); } } @@ -159,11 +315,33 @@ int main(int argc, char *argv[]) { int opt; - while ((opt = getopt(argc, argv, "q")) != -1) { + while ((opt = getopt(argc, argv, "qr:t:")) != -1) { switch (opt) { 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]);