dhcp/sock.cpp

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

#include <netinet/in.h>
#include <linux/filter.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <linux/if.h>
#include <sys/socket.h>
#include <sys/ioctl.h>

#include "sock.h"

namespace sock {

  //////////////////////////////////////////////////////////////////////
  // socket filter
  //////////////////////////////////////////////////////////////////////

  void attach_udp_filter(int sock, uint32_t destination_port)
  {
    struct sock_filter filter[] = {
      { 0x28,  0,  0, 0x0000000c },
      { 0x15,  0,  8, 0x00000800 },
      { 0x30,  0,  0, 0x00000017 },
      { 0x15,  0,  6, 0x00000011 },
      { 0x28,  0,  0, 0x00000014 },
      { 0x45,  4,  0, 0x00001fff },
      { 0xb1,  0,  0, 0x0000000e },
      { 0x48,  0,  0, 0x00000010 },
      { 0x15,  0,  1, destination_port },
      { 0x06,  0,  0, 0x00040000 },
      { 0x06,  0,  0, 0000000000 },
    };

    struct sock_fprog fprog = {
      .len = sizeof(filter) / sizeof(filter[0]),
      .filter = filter,
    };
    int ret = setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, (sizeof (fprog)));
    if (ret < 0) {
      perror("setsockopt(SO_ATTACH_FILTER)");
      exit(EXIT_FAILURE);
    }
  }

  //////////////////////////////////////////////////////////////////////
  // fake socket
  //////////////////////////////////////////////////////////////////////

  int create_fake_socket(int port) {
    int sf = socket(AF_INET, SOCK_DGRAM, 0);
    if (sf < 0) {
      perror("socket");
      exit(EXIT_FAILURE);
    }

    // this has the effect of inhibiting linux's generation of icmp
    // "destination unreachable" packets
    struct sockaddr_in bind_addr_in = {
      .sin_family = AF_INET,
      .sin_port   = htons(port),
      .sin_addr   = { .s_addr = INADDR_ANY },
    };
    int bret_in = bind(sf, (struct sockaddr *)&bind_addr_in, (sizeof (struct sockaddr_in)));
    if (bret_in < 0) {
      perror("bind in");
      exit(EXIT_FAILURE);
    }

    return sf;
  }
  //////////////////////////////////////////////////////////////////////
  // create
  //////////////////////////////////////////////////////////////////////

  int create(int port)
  {
    int sock = socket(AF_PACKET, SOCK_RAW, IPPROTO_RAW);
    if (sock < 0) {
      perror("socket");
      exit(EXIT_FAILURE);
    }

    //////////////////////////////////////////////////////////////////////
    // broadcast
    //////////////////////////////////////////////////////////////////////
    int enable = 1;
    int ret = setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &enable, (sizeof (int)));
    if (ret < 0) {
      perror("setsockopt(SO_BROADCAST)");
      exit(EXIT_FAILURE);
    }

    //////////////////////////////////////////////////////////////////////
    // dhcp_filter
    //////////////////////////////////////////////////////////////////////
    attach_udp_filter(sock, port);

    //////////////////////////////////////////////////////////////////////
    // fake socket
    //////////////////////////////////////////////////////////////////////
    create_fake_socket(port);

    return sock;
  }


  //////////////////////////////////////////////////////////////////////
  // bind
  //////////////////////////////////////////////////////////////////////

  void bind_interface(int sock, int ifa_index)
  {
    struct sockaddr_ll bind_addr_ll = {
      .sll_family   = AF_PACKET,
      .sll_protocol = htons(ETH_P_IP),
      .sll_ifindex  = ifa_index,
    };
    int bret_ll = bind(sock, (struct sockaddr *)&bind_addr_ll, (sizeof (struct sockaddr_ll)));
    if (bret_ll < 0) {
      perror("bind ll");
      exit(EXIT_FAILURE);
    }
  }

  //////////////////////////////////////////////////////////////////////
  // send
  //////////////////////////////////////////////////////////////////////

  void send(int sock,
            int interface_index,
            void * buf,
            int length,
            uint8_t const * eth_destination_address)
  {
    struct sockaddr_ll socket_address;
    socket_address.sll_ifindex = interface_index;
    socket_address.sll_halen = ETH_ALEN;
    memcpy(socket_address.sll_addr, eth_destination_address, 6);
    int flags = 0;
    int sret = sendto(sock, buf, length, flags,
                      (struct sockaddr *)&socket_address, sizeof(struct sockaddr_ll));
    if (sret < 0) {
      perror("sendto");
      exit(EXIT_FAILURE);
    }
  }

  //////////////////////////////////////////////////////////////////////
  // interface index
  //////////////////////////////////////////////////////////////////////

  int interface_index(int sock, char const * interface_name)
  {
    struct ifreq if_index;
    memset(&if_index, 0, (sizeof (struct ifreq)));
    strncpy(if_index.ifr_name, interface_name, IFNAMSIZ - 1);
    int iret = ioctl(sock, SIOCGIFINDEX, &if_index);
    if (iret < 0) {
      perror("ioctl(SIOCGIFINDEX)");
      exit(EXIT_FAILURE);
    }
    int ifa_index = if_index.ifr_ifindex;
    printf("interface index %d\n", ifa_index);
    return ifa_index;
  }

  //////////////////////////////////////////////////////////////////////
  // interface address
  //////////////////////////////////////////////////////////////////////

  void interface_address(int sock, char const * interface_name, uint8_t * hardware_address)
  {
    struct ifreq if_hwaddr;
    memset(&if_hwaddr, 0, sizeof(struct ifreq));
    strncpy(if_hwaddr.ifr_name, interface_name, IFNAMSIZ - 1);
    int hret = ioctl(sock, SIOCGIFHWADDR, &if_hwaddr);
    if (hret < 0) {
      perror("ioctl(SIOCGIFHWADDR)");
      exit(EXIT_FAILURE);
    }
    printf("hardware address %02x:%02x:%02x:%02x:%02x:%02x\n",
           (uint8_t)if_hwaddr.ifr_hwaddr.sa_data[0], (uint8_t)if_hwaddr.ifr_hwaddr.sa_data[1],
           (uint8_t)if_hwaddr.ifr_hwaddr.sa_data[2], (uint8_t)if_hwaddr.ifr_hwaddr.sa_data[3],
           (uint8_t)if_hwaddr.ifr_hwaddr.sa_data[4], (uint8_t)if_hwaddr.ifr_hwaddr.sa_data[5]);

    memcpy(hardware_address, if_hwaddr.ifr_hwaddr.sa_data, 6);
  }
}