Send / Receive raw Ethernet frames with custom EtherType in Linux

ethcom.c

/* -*- mode: C; c-file-style: "gnu"; indent-tabs-mode: nil; -*-
   vim: set autoindent expandtab shiftwidth=2 softtabstop=2 tabstop=2: */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <getopt.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <net/ethernet.h>
#include <net/if.h>
#include <arpa/inet.h>

#define ETHER_TYPE 0x80ab /* custom type */

#define BUF_SIZE (ETH_FRAME_LEN)

static char broadcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };

static void
print_usage (const char *progname)
{
  fprintf (stderr, "usage: %s [-l] [-i device] [-d dest-addr] msg\n", progname);
}

int
main (int    argc,
      char **argv)
{
  char *if_name;
  int listen;
  char *msg;
  int sock;
  int if_index;
  uint8_t if_addr[ETH_ALEN];
  uint8_t dest_addr[ETH_ALEN];
  size_t send_len;
  char buf[BUF_SIZE];
  int i;

  if_name = "eth0";
  memcpy (dest_addr, broadcast_addr, ETH_ALEN);
  listen = 0;
  msg = "Hello";

  {
    int opt;

    while ((opt = getopt (argc, argv, "li:d:")) != -1)
      {
        switch (opt)
          {
          case 'l':
            listen = 1;
            break;
          case 'i':
            if_name = optarg;
            break;
          case 'd':
            {
              int mac[ETH_ALEN];

              if (ETH_ALEN != sscanf (optarg,
                                      "%02x:%02x:%02x:%02x:%02x:%02x",
                                      &mac[0],
                                      &mac[1],
                                      &mac[2],
                                      &mac[3],
                                      &mac[4],
                                      &mac[5]))
                {
                  print_usage (argv[0]);
                  return EXIT_FAILURE;
                }
              for (i = 0; i < ETH_ALEN; i++)
                dest_addr[i] = mac[i];
            }
            break;
          default: /* '?' */
            print_usage (argv[0]);
            return EXIT_FAILURE;
          }
      }

    if (optind < argc)
      msg = argv[optind];
  }

  /* Create the AF_PACKET socket. */
  sock = socket (AF_PACKET, SOCK_RAW, htons (ETHER_TYPE));
  if (sock < 0)
    perror ("socket()");
  
  /* Get the index number and MAC address of ethernet interface. */
  {
    struct ifreq ifr;

    memset (&ifr, 0, sizeof (ifr));
    strncpy (ifr.ifr_name, if_name, IFNAMSIZ - 1);

    if (ioctl (sock, SIOCGIFINDEX, &ifr) < 0)
      perror ("SIOCGIFINDEX");
    if_index = ifr.ifr_ifindex;

    if (ioctl (sock, SIOCGIFHWADDR, &ifr) < 0)
      perror ("SIOCGIFHWADDR");
    memcpy (if_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
  }

  if (listen)
    {
      struct ifreq ifr;
      int s;

      memset (&ifr, 0, sizeof (ifr));
      strncpy (ifr.ifr_name, if_name, IFNAMSIZ - 1);

      /* Set interface to promiscuous mode. */
      if (ioctl (sock, SIOCGIFFLAGS, &ifr) < 0)
        perror ("SIOCGIFFLAGS");
      ifr.ifr_flags |= IFF_PROMISC;
      if (ioctl (sock, SIOCSIFFLAGS, &ifr) < 0)
        perror ("SIOCSIFFLAGS");

      /* Allow the socket to be reused. */
      s = 1;
      if (setsockopt (sock, SOL_SOCKET, SO_REUSEADDR, &s, sizeof (s)) < 0)
        {
          perror ("SO_REUSEADDR");
          close (sock);
          return EXIT_FAILURE;
        }

      /* Bind to device. */
      if (setsockopt (sock, SOL_SOCKET, SO_BINDTODEVICE, if_name, IFNAMSIZ - 1) < 0)
        {
          perror ("SO_BINDTODEVICE");
          close (sock);
          return EXIT_FAILURE;
        }

      while (1)
        {
          struct ether_header *eh = (struct ether_header *) buf;
          ssize_t received;
          char *p;

          received = recvfrom (sock, buf, BUF_SIZE, 0, NULL, NULL);
          if (received <= 0)
            break;

          /* Receive only destination address is broadcast or me. */
          if (memcmp (eh->ether_dhost, if_addr, ETH_ALEN) != 0 &&
              memcmp (eh->ether_dhost, broadcast_addr, ETH_ALEN) != 0)
            continue;

          fprintf (stdout,
                   "%02x:%02x:%02x:%02x:%02x:%02x -> %02x:%02x:%02x:%02x:%02x:%02x ",
                   eh->ether_shost[0],
                   eh->ether_shost[1],
                   eh->ether_shost[2],
                   eh->ether_shost[3],
                   eh->ether_shost[4],
                   eh->ether_shost[5],
                   eh->ether_dhost[0],
                   eh->ether_dhost[1],
                   eh->ether_dhost[2],
                   eh->ether_dhost[3],
                   eh->ether_dhost[4],
                   eh->ether_dhost[5]);
      
          received -= sizeof (*eh);
          p = buf + sizeof (*eh);
          for (i = 0; i < received; i++)
            fputc (p[i], stdout);

          fputc ('\n', stdout);
        }

      close (sock);

      return 0;
    }

  memset (buf, 0, BUF_SIZE);
  
  /* Construct ehternet header. */
  {
    struct ether_header *eh;

    /* Ethernet header */
    eh = (struct ether_header *) buf;
    memcpy (eh->ether_shost, if_addr, ETH_ALEN);
    memcpy (eh->ether_dhost, dest_addr, ETH_ALEN);
    eh->ether_type = htons (ETHER_TYPE);

    send_len = sizeof (*eh);
  }

  /* Fill the packet data. */
  for (i = 0; msg[i] != '\0'; i++)
    buf[send_len++] = msg[i];

  /* Fill the destination address and send it. */
  {
    struct sockaddr_ll sock_addr;

    sock_addr.sll_ifindex = if_index;
    sock_addr.sll_halen = ETH_ALEN;
    memcpy (sock_addr.sll_addr, dest_addr, ETH_ALEN);

    if (sendto (sock, buf, send_len, 0,
                (struct sockaddr *) &sock_addr, sizeof (sock_addr)) < 0)
      perror ("sendto()");
  }

  close (sock);
  
  return 0;
}

Note that this doesn't quite work as you'd expect. The issue is that SO_BINDTODEVICE doesn't work on packet sockets (AF_PACKET) - see the comment on this option in man 7 socket, which recommends using bind(). You end up receiving raw packets on every interface. It's quite difficult to discover this when writing an example like this, because you've probably only got one interface on your system receiving these packets anyway. Unfortunately, bind() doesn't seem to do the right thing when used with a custom ethertype either (at least on Linux 5.11).