+enum in2out in_recvmmsg(struct in_ctx *ctx, struct can_frame *cf)
+{
+ int ret;
+
+ if (ctx->current >= ctx->received) {
+ ret = CHECK(recvmmsg(ctx->s, ctx->msgs, MMSG_MAX, MSG_WAITFORONE, NULL));
+ if (nonblocking && ret == -1 && errno == EAGAIN)
+ return NOP;
+ ctx->received = ret;
+ ctx->current = 0;
+ }
+
+ memcpy(cf, ctx->bufs[ctx->current], sizeof(struct can_frame));
+ ctx->current++;
+
+ return (ctx->current < ctx->received) ? STORE_ONLY : SEND;
+}
+
+void init_recvmmg(struct in_ctx *ctx)
+{
+ int i;
+
+ init_read(ctx);
+ ctx->in_fn = in_recvmmsg;
+ ctx->current = 0;
+ memset(ctx->msgs, 0, sizeof(ctx->msgs));
+ memset(ctx->iovecs, 0, sizeof(ctx->iovecs));
+ for (i = 0; i < MMSG_MAX; i++) {
+ ctx->iovecs[i].iov_base = ctx->bufs[i];
+ ctx->iovecs[i].iov_len = sizeof(struct can_frame);
+ ctx->msgs[i].msg_hdr.msg_iov = &ctx->iovecs[i];
+ ctx->msgs[i].msg_hdr.msg_iovlen = 1;
+ }
+}
+
+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;
+
+ 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;
+ *cf = *cf_mmap;
+ hdr->tp_status = 0;
+ ctx->current = (ctx->current + 1) % FRAME_NR;
+ hdr = ctx->ptr + ctx->current*FRAME_SIZE;
+ return (hdr->tp_status == TP_STATUS_KERNEL) ? SEND : STORE_ONLY;
+}
+
+
+void init_packet_rx(struct in_ctx *ctx)
+{
+ int s;
+ struct sockaddr_ll my_addr;
+ struct ifreq ifr;
+
+ s = CHECK(socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)));
+
+ if (busy_poll_us) {
+ CHECK(setsockopt(s, SOL_SOCKET, SO_BUSY_POLL,
+ &busy_poll_us, sizeof(busy_poll_us)));
+ }
+
+ 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;
+}
+