From: Michal Sojka Date: Sat, 11 Jan 2014 20:25:58 +0000 (+0100) Subject: Add PF_PACKET + mmap() support for RX in ugw X-Git-Tag: fix-allnoconfig~96 X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/commitdiff_plain/aa3c50e2235871a58e803fbe4ea70fb0172fe1ee?ds=sidebyside Add PF_PACKET + mmap() support for RX in ugw --- diff --git a/ugw/ppc/S50ugw b/ugw/ppc/S50ugw index 75d5fdf..f4147b6 100644 --- a/ugw/ppc/S50ugw +++ b/ugw/ppc/S50ugw @@ -5,6 +5,4 @@ ip link set up dev can0 ip link set can1 type can bitrate 1000000 ip link set up dev can1 -echo Starting UGW - -ugw +ugw -r mmapbusy & diff --git a/ugw/ugw.c b/ugw/ugw.c index a3395c3..23d438c 100644 --- a/ugw/ugw.c +++ b/ugw/ugw.c @@ -15,14 +15,25 @@ #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 { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ; enum { WRITE } out_method = WRITE; bool quiet = false; @@ -31,6 +42,9 @@ bool quiet = false; 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); }; @@ -58,7 +72,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 +87,66 @@ void init_read(struct in_ctx *ctx) ctx->in_fn = in_read; } +int 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; + return 0; +} + + +void init_packet_rx(struct in_ctx *ctx) +{ + int s; + struct sockaddr_ll my_addr; + struct ifreq ifr; + + s = CHECK(socket(PF_PACKET, 1 ? SOCK_RAW : SOCK_DGRAM, 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 +165,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); @@ -109,7 +183,13 @@ void init_write(struct out_ctx *ctx) void init_in(struct in_ctx *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); @@ -145,7 +225,7 @@ void gw() init_in(&ic); init_out(&oc); - VERBOSE("UGW started"); + VERBOSE("UGW started\n"); while (1) { in(&ic, &cf); @@ -159,11 +239,23 @@ int main(int argc, char *argv[]) { int opt; - while ((opt = getopt(argc, argv, "q")) != -1) { + while ((opt = getopt(argc, argv, "qr:")) != -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; default: /* '?' */ fprintf(stderr, "Usage: %s [in_interface out_interface]\n", argv[0]);