Add PF_PACKET + mmap() support for RX in ugw
authorMichal Sojka <sojkam1@fel.cvut.cz>
Sat, 11 Jan 2014 20:25:58 +0000 (21:25 +0100)
committerMichal Sojka <sojkam1@fel.cvut.cz>
Sat, 11 Jan 2014 20:25:58 +0000 (21:25 +0100)
ugw/ppc/S50ugw
ugw/ugw.c

index 75d5fdf..f4147b6 100644 (file)
@@ -5,6 +5,4 @@ ip link set up dev can0
 ip link set can1 type can bitrate 1000000
 ip link set up dev can1
 
 ip link set can1 type can bitrate 1000000
 ip link set up dev can1
 
-echo Starting UGW
-
-ugw
+ugw -r mmapbusy &
index a3395c3..23d438c 100644 (file)
--- a/ugw/ugw.c
+++ b/ugw/ugw.c
 #include <stdio.h>
 #include <time.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <time.h>
 #include <stdbool.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <poll.h>
+#include <sys/mman.h>
+#include <arpa/inet.h>
+
+#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 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";
 
 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;
 
 enum { WRITE }                out_method = WRITE;
 bool quiet = false;
 
@@ -31,6 +42,9 @@ bool quiet = false;
 struct in_ctx {
        int s;
        int for_out;
 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);
 };
 
        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));
 
 
        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);
        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;
 }
 
        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));
 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));
 
 
        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);
        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) {
 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);
        default:
                fprintf(stderr, "Unknown \"in method\" %d\n", in_method);
                exit(1);
@@ -145,7 +225,7 @@ void gw()
        init_in(&ic);
        init_out(&oc);
 
        init_in(&ic);
        init_out(&oc);
 
-       VERBOSE("UGW started");
+       VERBOSE("UGW started\n");
 
        while (1) {
                in(&ic, &cf);
 
        while (1) {
                in(&ic, &cf);
@@ -159,11 +239,23 @@ int main(int argc, char *argv[])
 {
        int opt;
 
 {
        int opt;
 
-       while ((opt = getopt(argc, argv, "q")) != -1) {
+       while ((opt = getopt(argc, argv, "qr:")) != -1) {
                switch (opt) {
                case 'q':
                        quiet = true;
                        break;
                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]);
                default: /* '?' */
                        fprintf(stderr, "Usage: %s [in_interface out_interface]\n",
                                argv[0]);