pingxelflut/raw_ethernet.c

/*
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 */

#include <arpa/inet.h>
#include <linux/if_ether.h>
#include <linux/if_packet.h>
#include <linux/ip.h>
#include <linux/ipv6.h>
#include <linux/udp.h>
#include <net/if.h>
#include <netinet/ether.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

static uint8_t dest_mac[6] = {0x08, 0x11, 0x96, 0x91, 0xb6, 0xac};
/* static uint8_t dest_mac[6] = {0x00, 0x21, 0xcc, 0xbf, 0xe6, 0xae}; */

#define ETHER_TYPE 0x0800

/* #define DEFAULT_IF	"enp0s25" */
#define DEFAULT_IF "wlp3s0"
#define BUF_SIZ 1024

int main(int argc, char *argv[]) {
  char sender[INET6_ADDRSTRLEN];
  char receiver[INET6_ADDRSTRLEN];
  int sockfd, ret;
  int sockopt;
  ssize_t numbytes;
  struct ifreq ifopts; /* set promiscuous mode */
  struct ifreq if_ip;  /* get ip addr */
  struct sockaddr_storage their_addr;
  struct sockaddr_storage our_addr;
  uint8_t buf[BUF_SIZ];
  char ifName[IFNAMSIZ];

  /* Get interface name */
  if (argc > 1)
    strcpy(ifName, argv[1]);
  else
    strcpy(ifName, DEFAULT_IF);

  /* Header structures */
  struct ether_header *eh = (struct ether_header *)buf;
  struct iphdr *iph = (struct iphdr *)(buf + sizeof(struct ether_header));
  struct udphdr *udph = (struct udphdr *)(buf + sizeof(struct iphdr) +
                                          sizeof(struct ether_header));

  memset(&if_ip, 0, sizeof(struct ifreq));

  /* Open PF_PACKET socket, listening for EtherType ETHER_TYPE */
  if ((sockfd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) == -1) {
    perror("listener: socket");
    return -1;
  }

  /* Set interface to promiscuous mode - do we need to do this every time? */
  strncpy(ifopts.ifr_name, ifName, IFNAMSIZ - 1);
  ioctl(sockfd, SIOCGIFFLAGS, &ifopts);
  ifopts.ifr_flags |= IFF_PROMISC;
  ioctl(sockfd, SIOCSIFFLAGS, &ifopts);
  /* Allow the socket to be reused - incase connection is closed prematurely */
  if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof sockopt) ==
      -1) {
    perror("setsockopt");
    close(sockfd);
    exit(EXIT_FAILURE);
  }
  /* Bind to device */
  if (setsockopt(sockfd, SOL_SOCKET, SO_BINDTODEVICE, ifName, IFNAMSIZ - 1) ==
      -1) {
    perror("SO_BINDTODEVICE");
    close(sockfd);
    exit(EXIT_FAILURE);
  }

  while (1) {
    numbytes = recvfrom(sockfd, buf, BUF_SIZ, 0, NULL, NULL);
    /* Check the packet is for me */
    if (eh->ether_dhost[0] == dest_mac[0] &&
        eh->ether_dhost[1] == dest_mac[1] &&
        eh->ether_dhost[2] == dest_mac[2] &&
        eh->ether_dhost[3] == dest_mac[3] &&
        eh->ether_dhost[4] == dest_mac[4] &&
        eh->ether_dhost[5] == dest_mac[5]) {
      printf("listener: got packet %lu bytes\n", numbytes);
      printf("Correct destination MAC address\n");
    } else {
      /* printf("Wrong destination MAC: %x:%x:%x:%x:%x:%x\n", */
      /* 				eh->ether_dhost[0], */
      /* 				eh->ether_dhost[1], */
      /* 				eh->ether_dhost[2], */
      /* 				eh->ether_dhost[3], */
      /* 				eh->ether_dhost[4], */
      /* 				eh->ether_dhost[5]); */
      ret = -1;
      continue;
    }

    if (iph->version == 4) {
			continue;
      /* Get source IP */
      ((struct sockaddr_in *)&their_addr)->sin_addr.s_addr = iph->saddr;
      inet_ntop(AF_INET, &((struct sockaddr_in *)&their_addr)->sin_addr, sender,
                sizeof sender);

      /* Get dest IP */
      ((struct sockaddr_in *)&our_addr)->sin_addr.s_addr = iph->daddr;
      inet_ntop(AF_INET, &((struct sockaddr_in *)&our_addr)->sin_addr, receiver,
                sizeof receiver);
    } else {
      /* Get source IP */
      memcpy(&((struct sockaddr_in6 *)&their_addr)->sin6_addr.s6_addr,
             &((struct ipv6hdr *)iph)->saddr, sizeof(struct in6_addr));
      inet_ntop(AF_INET6, &((struct sockaddr_in *)&their_addr)->sin_addr,
                sender, sizeof sender);

      /* Get dest IP */
      memcpy(&((struct sockaddr_in6 *)&our_addr)->sin6_addr.s6_addr,
             &((struct ipv6hdr *)iph)->daddr, sizeof(struct in6_addr));
      inet_ntop(AF_INET6, &((struct sockaddr_in6 *)&our_addr)->sin6_addr,
                receiver, sizeof receiver);
    }
    /* Look up my device IP addr if possible */
    strncpy(if_ip.ifr_name, ifName, IFNAMSIZ - 1);
    /* if (ioctl(sockfd, SIOCGIFADDR, &if_ip) >= 0) { /\* if we can't check then
     * don't *\/ */
    printf("Source IP: %s\n My IP: %s\n", sender, receiver);
    /* ignore if I sent it */
    if (strcmp(sender,
               inet_ntoa(((struct sockaddr_in *)&if_ip.ifr_addr)->sin_addr)) ==
        0) {
      printf("but I sent it :(\n");
      ret = -1;
      continue;
    }
    /* } */

    /* UDP payload length */
    ret = ntohs(udph->len) - sizeof(struct udphdr);

    /* Print packet */
    /* printf("\tData:"); */
    /* for (i=0; i<numbytes; i++) printf("%02x:", buf[i]); */
    /* printf("\n"); */
  }

  close(sockfd);
  return ret;
}