Open raw socket specified with both PACKET_TX_RING and PACKET_RX_RING and send and receive EtherCAT packet.

txrx-mmap-ecat.c

/*
* This application tests whether an ethernet port can correctly send and receive
* data with both PACKET_TX_RING and PACKET_RX_RING enabled.
* 
* This application uses EtherCAT packets and is intended to communicate with an EtherCAT slave device
* connected to the eth0 port.  This core problem looks to be unrelated to the use of EtherCAT; it's just
* that communicating with a single device that echos (or almost echos) data is easier to write a test for.
* 
* The test contains 2 sub-tests:
* (1) Send and receive data using PACKET_RX_RING but without PACKET_TX_RING.  (named 'Tx-Send' in the test output)
* (2) Send and receive data using both PACKET_RX_RING and PACKET_TX_RING.  (named 'Tx-MMAP' in the test output)
* 
* On x86-64 machines with Ubuntu 23.04, the test generates the following output
* | using interface: eth0
* | Rx [30]:
* | Rx [60]:
* | OK w/Tx-Send
* | Rx [30]:
* | Rx [60]:
* | OK w/Tx-MMAP
* 
* The above is the expected output.
* 
* On a Raspberry Pi CM4 with Ubuntu 23.04, the test generates the following output
 * | using interface: eth0
 * | Rx [30]:
 * | Rx [60]:
 * | OK w/Tx-Send
 * | Rx [30]:
 * | Failed w/Tx-MMAP
 * 
 * On some EtherCAT device, the first run of this program will result in data not being sent back correctly
 * reporting that both Tx-Send and Tx-MMAP failed.  Subsequent runs do show that Tx-Send succeeds but that Tx-MMAP fails.
 */

#include <stdarg.h>
#define _GNU_SOURCE
#define __USE_GNU
#include <pthread.h>
#include <sched.h>
#include <inttypes.h>

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <unistd.h>

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <poll.h>

#include <arpa/inet.h>
#include <netinet/if_ether.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/stat.h>

#include <linux/if.h>
#include <linux/if_packet.h>

/// The number of frames in the ring
//  This number is not set in stone. Nor are block_size, block_nr or frame_size
#define CONF_RING_FRAMES 16
#define FRAME_SIZE 2048
#define CONF_DEVICE "eth0"

#define SOCK_PROTOCOL(ringtype) htons(0x88a4)
#define SOCKADDR_PROTOCOL htons(0x88a4)

/// Offset of data from start of frame
#define TX_DATA_OFFSET TPACKET_ALIGN(sizeof(struct tpacket2_hdr))
#define RX_DATA_OFFSET TX_DATA_OFFSET + 34

/// (unimportant) macro for loud failure
#define RETURN_ERROR(lvl, msg) \
    do                         \
    {                          \
        fprintf(stderr, msg);  \
        return lvl;            \
    } while (0);

void HandleError(const char* msg, int error)
{
    if (error != 0)
    {
        errno = error;
        perror(msg);
        _exit(error);
    }
}

void SetAffinity(int8_t cpu)
{
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu, &cpuset);

    HandleError("pthread_set_affinity_np", pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset));
}

static struct sockaddr_ll txring_daddr;
static struct sockaddr_ll dest_daddr;
char*					  Ethernet_ifname = CONF_DEVICE;

/// create a linklayer destination address
//  @param ringdev is a link layer device name, such as "eth0"
static int init_ring_daddr(int fd, const char* ringdev, const int ringtype)
{
    struct ifreq ifr;
    int			 ifindex;

    // get device index
    strcpy(ifr.ifr_name, ringdev);
    if (ioctl(fd, SIOCGIFINDEX, &ifr))
    {
        perror("ioctl");
        return -1;
    }
    ifindex = ifr.ifr_ifindex;
    memset(&txring_daddr, 0, sizeof(txring_daddr));

    txring_daddr.sll_family	  = AF_PACKET;
    txring_daddr.sll_protocol = SOCKADDR_PROTOCOL;
    txring_daddr.sll_ifindex  = ifindex;
    txring_daddr.sll_halen	  = ETH_ALEN;  // e4:5f:01:6e:de:3d
    memset(&txring_daddr.sll_addr, 0xFF, ETH_ALEN);

    memcpy(&dest_daddr, &txring_daddr, sizeof(dest_daddr));

    return 0;
}

/// Initialize a packet socket ring buffer
//  @param ringtype is one of PACKET_RX_RING or PACKET_TX_RING
static char* init_packetsock_ring(int fd, int ringtype, int tx_mmap)
{
    struct tpacket_req tp;
    char*			   ring;
    int				   packet_version = TPACKET_V2;

    if (setsockopt(fd, SOL_PACKET, PACKET_VERSION, &packet_version, sizeof(packet_version)))
    {
        perror("setsockopt packet version");
        return NULL;
    }

    // tell kernel to export data through mmap()ped ring
    tp.tp_block_size = getpagesize();
    tp.tp_frame_size = FRAME_SIZE;
    tp.tp_frame_nr	 = CONF_RING_FRAMES;
    tp.tp_block_nr	 = (tp.tp_frame_nr * tp.tp_frame_size) / tp.tp_block_size;

    {
        if (init_ring_daddr(fd, Ethernet_ifname, ringtype))
            return NULL;
    }

    if (ringtype == PACKET_TX_RING & !tx_mmap)
    {
        return NULL;
    }

    if (setsockopt(fd, SOL_PACKET, ringtype, (void*)&tp, sizeof(tp)))
        RETURN_ERROR(NULL, "setsockopt() ring\n");

    // open ring
    ring = mmap(0, tp.tp_block_size * tp.tp_block_nr, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
    if (ring == MAP_FAILED)
        RETURN_ERROR(NULL, "mmap()\n");

    return ring;
}

/// Create a packet socket. If param ring is not NULL, the buffer is mapped
//  @param ring will, if set, point to the mapped ring on return
//  @return the socket fd
static int init_packetsock(char** ring, int ringtype, int tx_mmap)
{
    int fd;

    // open packet socket
    fd = socket(PF_PACKET, SOCK_RAW, SOCK_PROTOCOL(ringtype));
    if (fd < 0)
        RETURN_ERROR(-1, "Root priliveges are required\nsocket() rx. \n");

    if (ring)
    {
        *ring = init_packetsock_ring(fd, ringtype, tx_mmap);

        if (!tx_mmap)
            return fd;

        if (!*ring)
        {
            // printf("Closing fd\n");
            close(fd);
            return -1;
        }
    }

    return fd;
}

static int exit_packetsock(int fd, char* ring, int tx_mmap)
{
    if (tx_mmap && munmap(ring, CONF_RING_FRAMES * FRAME_SIZE))
    {
        perror("munmap");
        return 1;
    }

    if (close(fd))
    {
        perror("close");
        return 1;
    }

    return 0;
}

/// transmit a packet using packet ring
//  NOTE: for high rate processing try to batch system calls,
//        by writing multiple packets to the ring before calling send()
//
//  @param pkt is a packet from the network layer up (e.g., IP)
//  @return 0 on success, -1 on failure
static int process_tx(int fd, char* ring, const char* pkt, size_t pktlen, int offset, int tx_mmap)
{
    static int ring_offset = 0;

    struct tpacket2_hdr* header;
    struct pollfd		 pollset;
    char*				 off;
    int					 ret;

    if (tx_mmap)
    {
        // fetch a frame
        // like in the PACKET_RX_RING case, we define frames to be a page long,
        // including their header. This explains the use of getpagesize().
        header = (void*)ring + (ring_offset * FRAME_SIZE);
        assert((((unsigned long)header) & (FRAME_SIZE - 1)) == 0);
        while (header->tp_status != TP_STATUS_AVAILABLE)
        {
            // if none available: wait on more data
            pollset.fd		= fd;
            pollset.events	= POLLOUT;
            pollset.revents = 0;
            ret				= poll(&pollset, 1, 1 /* don't hang */);
            if (ret < 0)
            {
                if (errno != EINTR)
                {
                    perror("poll");
                    return -1;
                }
                return 0;
            }
        }

        // fill data
        off = ((void*)header) + TX_DATA_OFFSET + offset;
        memcpy(off, pkt, pktlen);

        // fill header
        header->tp_len	  = pktlen;
        header->tp_status = TP_STATUS_SEND_REQUEST;

        // increase consumer ring pointer
        ring_offset = (ring_offset + 1) & (CONF_RING_FRAMES - 1);

        if (send(fd, NULL, 0, 0) < 0)
        // if (sendto(fd, NULL, 0, 0, (struct sockaddr*)&dest_daddr, sizeof(dest_daddr)) < 0)
        {
            perror("sendto");
            return -1;
        }
    }
    else
    {
        off = (void*)pkt;

        if (sendto(fd, off, pktlen, 0, (struct sockaddr*)&dest_daddr, sizeof(dest_daddr)) < 0)
        {
            perror("sendto");
            return -1;
        }
    }

    // printf("Tx:%d\n",ring_offset);fflush(stdout);

    return 0;
}

static void* process_rx(const int fd, char* rx_ring, int* len)
{
    volatile struct tpacket2_hdr* header;
    struct pollfd				  pollset;
    int							  ret;

    for (int i = 0; i < CONF_RING_FRAMES; i++)
    {
        // fetch a frame
        header = (void*)rx_ring + (i * FRAME_SIZE);
        assert((((unsigned long)header) & (FRAME_SIZE - 1)) == 0);
        if (header->tp_status & TP_STATUS_USER)
        {
            if (header->tp_status & TP_STATUS_COPY)
            {
                printf("copy\n");
                continue;
            }
            *len = header->tp_len;
            return (void*)header;
        }
    }
    return NULL;
}

// Release the slot back to the kernel
static void process_rx_release(char* packet)
{
    volatile struct tpacket2_hdr* header = (struct tpacket2_hdr*)packet;
    header->tp_status					 = TP_STATUS_KERNEL;
}

static void rx_flush(void* ring)
{
    for (int i = 0; i < CONF_RING_FRAMES; i++)
    {
        volatile struct tpacket2_hdr* hdr = ring + (i * FRAME_SIZE);
        hdr->tp_status					  = TP_STATUS_KERNEL;
    }
}

// e4:5f:01:6e:de:3d
static unsigned char EcatPacket[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00,
                                     0x00, 0x00, 0x88, 0xa4, 0x0e, 0x10, 0x07, 0x80, 0x00, 0x00,
                                     0x30, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

int test_tx(int tx_mmap)
{
    int	  status = 1;
    char* txRing;
    int	  txFd;
    char *rxRing, *pkt;
    int	  rxFd;
    int	  len;

    rxFd = init_packetsock(&rxRing, PACKET_RX_RING, 1);
    if (rxFd < 0)
        return 1;

    txFd = init_packetsock(&txRing, PACKET_TX_RING, tx_mmap);
    if (txFd < 0)
        return 1;

    {
        struct sockaddr_ll rxring_daddr = txring_daddr;
        rxring_daddr.sll_protocol		= htons(ETH_P_ALL);
        rxring_daddr.sll_halen			= ETH_ALEN;	 // e4:5f:01:6e:de:3d
        memset(&rxring_daddr.sll_addr, 0xFF, ETH_ALEN);
        if (bind(rxFd, (struct sockaddr*)&rxring_daddr, sizeof(rxring_daddr)) != 0)
        {
            perror("bind");
            return -1;
        }
        if (bind(txFd, (struct sockaddr*)&rxring_daddr, sizeof(rxring_daddr)) != 0)
        {
            perror("bind");
            return -1;
        }
    }

    rx_flush(rxRing);

    {
        int	  offset = 0;
        char* pkt	 = NULL;

        rx_flush(rxRing);
        process_tx(txFd, txRing, EcatPacket, sizeof(EcatPacket), offset, tx_mmap);
        sleep(1);
        while (pkt = process_rx(rxFd, rxRing, &len))
        {
            printf("Rx [%d]:\n", len);
            process_rx_release(pkt);
        }
    }

    if (exit_packetsock(txFd, txRing, tx_mmap))
        return 1;

    if (exit_packetsock(rxFd, rxRing, 1))
        return 1;

    return len == 60 ? 0 : 1;
}

void test_tx_send()
{
    fprintf(stderr, "%s w/Tx-Send\n", test_tx(0) ? "Failed" : "OK");
}

void test_tx_mmap()
{
    fprintf(stderr, "%s w/Tx-MMAP\n", test_tx(1) ? "Failed" : "OK");
}

int main(int argc, char** argv)
{
    if (argc > 1)
        Ethernet_ifname = argv[1];

    printf("using interface: %s\n", Ethernet_ifname);
    test_tx_send();
    test_tx_mmap();
}