问用Linux原始套接字捕获PTP数据包

EN

#include <stdio.h>
#include <stdbool.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>

#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/if_packet.h>
#include <linux/filter.h>
#include <net/if.h>
#include <net/ethernet.h>
#include <arpa/inet.h>

#define PTP_IF_NAME               "enx503eaa33fc9d"
#define PACKET_SIZE               300
#define NUM_OF_BLOCKS_PER_LINE    30
#define SIZE_OF_BYYE_PRINT_BLOCK  3
#define SIZE_OF_PRINT_BUFFER      \
    ( ( NUM_OF_BLOCKS_PER_LINE * SIZE_OF_BYYE_PRINT_BLOCK ) + 1 )

#define PERROR(x, ...)  printf( x "stderr:%m\n", ##__VA_ARGS__) 
#define INFO(x, ...)    printf( x "\n", ##__VA_ARGS__)
#define ERROR(x, ...)   printf( x "\n", ##__VA_ARGS__)

int eventSock = -1;

void printBuffer(unsigned char* buffer, size_t len) {
    unsigned char outBuffer[SIZE_OF_PRINT_BUFFER];
    unsigned char* pOutBuffer = outBuffer;
    int usedBytes = 0;

    for (int i = 0; i < len; i++) {
        if ((usedBytes + SIZE_OF_BYYE_PRINT_BLOCK) > SIZE_OF_PRINT_BUFFER) {
            INFO("%s", outBuffer);
            memset(outBuffer, 0, sizeof(outBuffer));
            usedBytes = 0;
            pOutBuffer = outBuffer;
        }
        sprintf(pOutBuffer, "%02x ", buffer[i]);
        usedBytes += SIZE_OF_BYYE_PRINT_BLOCK;
        pOutBuffer += SIZE_OF_BYYE_PRINT_BLOCK;
    }
    INFO("%s", outBuffer);
}

int getInterfaceIndex(char *ifaceName) {

    int sockfd;
    struct ifreq ifr;

    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0) {
        ERROR("Could not retrieve interface index for %s", ifaceName);
        return -1;
    }

    memset(&ifr, 0, sizeof(ifr));
    strncpy(ifr.ifr_name, ifaceName, IFNAMSIZ);
    if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0) {
        PERROR("failed to request hardware address for %s", ifaceName);
        close(sockfd);
        return -1;
    }

    close(sockfd);
    return ifr.ifr_ifindex;
}

bool InitSocket(char *ifaceName) {
    int IfIndex;
    struct sockaddr_ll sll;

    IfIndex = getInterfaceIndex(ifaceName);
    if (IfIndex < 0) {
        return false;
    }

    /* create the socket */
    if ((eventSock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) < 0) {
        PERROR("failed to initialize raw socket");
        return false;
    }

    /* binding to the interface */
    memset(&sll, 0, sizeof(sll));
    sll.sll_family = AF_PACKET;
    sll.sll_ifindex = IfIndex;
    sll.sll_protocol = htons(ETH_P_ALL);

    /* Bind the socket to the interface */
    if (bind(eventSock, (struct sockaddr *) &sll, sizeof(sll)) < 0) {
        PERROR("failed to bind raw event socket");
        return false;
    }
    
    /* bpf bytecode to filter only PTP packets (ethertype 0x88f7)
     * obtained via "tcpdump -dd ether proto 0x88f7" */
//  struct sock_filter filter_ptp[] = {
//          { 0x28, 0, 0, 0x0000000c }, // ldh [12]
//          { 0x15, 0, 1, 0x000088f7 }, // jeq #0x88f7 jt 2 jf 3
//          { 0x06, 0, 0, 0x00040000 }, // ret #262144
//          { 0x06, 0, 0, 0x00000000 }, // ret #0
//  };
//
//  struct sock_fprog bpf_ptp = { .len = 4, .filter = filter_ptp, };
//
//  if (setsockopt(eventSock, SOL_SOCKET, SO_ATTACH_FILTER, &bpf_ptp,
//                 sizeof(bpf_ptp))) {
//      PERROR("failed attaching filter to raw event socket");
//      return false;
//  }
}

int waitForData(struct timeval * timeout, fd_set *readfds) {
    int ret, nfds;
    struct timeval tv, *tv_ptr;

    if (timeout) {
        tv.tv_sec = timeout->tv_sec;
        tv.tv_usec = timeout->tv_usec;
        tv_ptr = &tv;
    } else {
        tv_ptr = NULL;
    }

    FD_ZERO(readfds);
    nfds = 0;

    FD_SET(eventSock, readfds);
    nfds = eventSock;
    nfds++;

    ret = select(nfds, readfds, 0, 0, tv_ptr);
    return ret;
}

void netr(void) {
    ssize_t ret = 0;
    char buf[1024];
    struct msghdr msg;
    struct iovec vec[1];
    struct sockaddr_in from_addr;

    union {
        struct cmsghdr cm;
        char control[256];
    } cmsg_un;

    struct cmsghdr *cmsg;

    vec[0].iov_base = buf;
    vec[0].iov_len = PACKET_SIZE;

    memset(&msg, 0, sizeof(msg));
    memset(&from_addr, 0, sizeof(from_addr));
    memset(buf, 0, PACKET_SIZE);
    memset(&cmsg_un, 0, sizeof(cmsg_un));

    msg.msg_name = (caddr_t) & from_addr;
    msg.msg_namelen = sizeof(from_addr);
    msg.msg_iov = vec;
    msg.msg_iovlen = 1;
    msg.msg_control = cmsg_un.control;
    msg.msg_controllen = sizeof(cmsg_un.control);
    msg.msg_flags = 0;

    ret = recvmsg(eventSock, &msg, MSG_DONTWAIT);
    if (ret < 0) {
        PERROR("failed to receive message!");
    }

    INFO("received Bytes: %ld", ret);
    printBuffer(msg.msg_iov->iov_base, PACKET_SIZE);
}

int main(void) {
    int ret;
    fd_set readfds;
    struct timeval timeout = { .tv_sec = 1, .tv_usec = 0 };

    INFO("main started!!");

    InitSocket(PTP_IF_NAME);

    while (1) {
        INFO("waiting for event!!");
        ret = waitForData(&timeout, &readfds);
        INFO("select return = %d", ret);
        if (ret > 0) {
            if (FD_ISSET(eventSock, &readfds)) {
                netr();
            }
        }
    }
}