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
 
 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 <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]);