Add PF_PACKET + mmap() support for RX in ugw

diff --git a/ugw/ppc/S50ugw b/ugw/ppc/S50ugw
index 75d5fdfcbbbb9afcee401e2f60927345981c8fdf..f4147b697dcbb4c4d72fa8f9ce0d0ceaca834555 100644 (file)
--- 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 a3395c34b44a62fa2c02631dcf0f25510d6a69d8..23d438c865dc3a19ca954623d6f0d61bbad56853 100644 (file)
--- a/ugw/ugw.c
+++ b/ugw/ugw.c
@@ -15,14 +15,25 @@
 #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 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]);