Add mmap()-based TX method - it doesn't work (yet?)

diff --git a/ugw/ugw.c b/ugw/ugw.c
index 23d438c865dc3a19ca954623d6f0d61bbad56853..bb654cf7c8c790c11334a234c7ac25530aed6289 100644 (file)
--- a/ugw/ugw.c
+++ b/ugw/ugw.c
@@ -34,34 +34,42 @@
 char *devin = "can0";
 char *devout = "can1";
 enum { IN_READ, IN_RECVMMSG, IN_MMAP, IN_MMAPBUSY } in_method = IN_READ;
-enum { WRITE }                out_method = WRITE;
+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;
        void *ptr;
        int hdrlen;
        int current;
-       int (*in_fn)(struct in_ctx *ctx, struct can_frame *cf);
+       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)
@@ -87,7 +95,7 @@ void init_read(struct in_ctx *ctx)
        ctx->in_fn = in_read;
 }
 
-int in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
+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;
@@ -106,7 +114,8 @@ int in_packet_rx(struct in_ctx *ctx, struct can_frame *cf)
        *cf = *cf_mmap;
        hdr->tp_status = 0;
        ctx->current = (ctx->current + 1) % FRAME_NR;
-       return 0;
+       hdr = ctx->ptr + ctx->current*FRAME_SIZE;
+       return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY;
 }
 
 
@@ -116,7 +125,7 @@ void init_packet_rx(struct in_ctx *ctx)
        struct sockaddr_ll my_addr;
        struct ifreq ifr;
 
-       s = CHECK(socket(PF_PACKET, 1 ? SOCK_RAW : SOCK_DGRAM, htons(ETH_P_ALL)));
+       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)));
@@ -180,8 +189,74 @@ 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 IN_READ:
                init_read(in);
@@ -203,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);
@@ -228,8 +305,7 @@ void gw()
        VERBOSE("UGW started\n");
 
        while (1) {
-               in(&ic, &cf);
-               oc.from_in = ic.for_out;
+               oc.from_in = in(&ic, &cf);
                out(&oc, &cf);
        }
 }
@@ -239,7 +315,7 @@ int main(int argc, char *argv[])
 {
        int opt;
 
-       while ((opt = getopt(argc, argv, "qr:")) != -1) {
+       while ((opt = getopt(argc, argv, "qr:t:")) != -1) {
                switch (opt) {
                case 'q':
                        quiet = true;
@@ -256,6 +332,16 @@ int main(int argc, char *argv[])
                                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]);