#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <iostream.h>
#include <fstream.h>
#include <strings.h>
#include <sys/ioctl.h>
#include <linux/if_vlan.h>
#include <string.h>


#define MAX_HOSTNAME 256

// END   Copied from linux/net/802_1Q/vlanproc.h --BLG


static char* usage =
      "
Usage: add      [interface-name] [vlan_id]
       rem      [vlan-name]
       set_dflt [interface-name] [vlan_id]
       add_port [port-name]      [vlan_id]
       rem_port [port-name]      [vlan_id]
       send     [interface-name] [src_mac] [dest_mac] [data_type] [data_bytes]

* The [interface-name] is the name of the ethernet card that hosts
  the VLAN you are talking about.
* The port-name is the name of the physical interface that a VLAN
  may be attached to.
* The vlan_id is the identifier (0-4095) of the VLAN you are operating on.
* The src_mac will be put in the ethernet header as such. (Enter in HEX)
* The dest_mac will be put in the ethernet header as such. (Enther in HEX)
* The data_type corresponds to the protocol id of the ethernet packet.
  See include/linux/if_ether.h for possible types. (Enter in HEX)
* The data_bytes are the bytes of the packet payload.  (Enter in HEX)
Example of the send command:
   send vlan0000 08aabbccddee 08eeddccbbaa 8100 00112233445566778899aabbccdd
";

void show_usage() {
   printf(usage);
}

int hex_to_bytes(char* bytes, int bytes_length, char* hex_str) {
   int hlen = strlen(hex_str);
   int j = 0;
   char hex[3];
   char* stop; /* not used for any real purpose */

   hex[2] = 0;
   for (int i = 0; i<hlen; i++) {
      hex[0] = hex_str[i];
      i++;
      if (i >= hlen) {
         return j; /* done */
      }
      
      hex[1] = hex_str[i];
      bytes[j++] = (char)strtoul(hex, &stop, 16);
   }
   return j;
}


int main(int argc, char** argv) {
   int fd;
   struct vlan_ioctl_args if_request;
   
   char* cmd = NULL;
   char* if_name = NULL;
   char dest_mac[6];
   char src_mac[6];
   char data_bytes[1500];
   int data_bytes_len = 0;
   unsigned short data_type = 0x0800; /* IP packet */
   unsigned int vid = 0;

   char* conf_file_name = "/proc/net/vlan/config";

   memset(&if_request, 0, sizeof(struct vlan_ioctl_args));
   
   if ((argc < 3) || (argc > 7)) {
      show_usage();
      exit(1);
   }
   else {
      cmd = argv[1];
      if_name = argv[2];
      if (strlen(if_name) > 15) {
         cerr << "ERROR:  if_name must be 15 characters or less." << endl;
         exit(1);
      }

      if (strcasecmp(cmd, "send") == 0) {
         if (argc == 7) {
            int mac_len;
            char* stop; /* not really used */
            if ((mac_len = hex_to_bytes(src_mac, 6, argv[3])) != 6) {
               cerr << "ERROR: src_mac is wrong length: " << mac_len << endl;
               exit(5);
            }

            if ((mac_len = hex_to_bytes(dest_mac, 6, argv[4])) != 6) {
               cerr << "ERROR: dest_mac is wrong length: " << mac_len << endl;
               exit(5);
            }
            
            data_type = strtoul(argv[5], &stop, 16);
            data_bytes_len = hex_to_bytes(data_bytes, 1500, argv[6]);
         }
         else {
            show_usage();
            exit(5);
         }
      }
      else if (argc == 4) {
	vid = 0;
	sscanf(argv[3],"%i",&vid);
	/* vid =  strtoul(argv[3], NULL, 10); */
      }
   }
   
   // Open up the /proc/vlan/config
   if ((fd = open(conf_file_name, O_RDONLY)) < 0) {
      cerr << "ERROR:  Could not open /proc/vlan/config.\n";
      exit(3);
   }


   strcpy(if_request.dev1, if_name);
   if_request.u.VID = vid;

   /* add */
   if (strcasecmp(cmd, "add") == 0) {
      if (ioctl(fd, ADD_VLAN_IOCTL, &if_request) < 0) {
         cerr << "ERROR: trying to add VLAN #" << vid << " to IF -:"
              << if_name << ":-  error: " << strerror(errno) << endl;
      }
      else {
         cout << "Added VLAN with VID == " << vid << " to IF -:"
              << if_name << ":-" << endl;
      }
   }//if
   else if (strcasecmp(cmd, "rem") == 0) {
      if (ioctl(fd, DEL_VLAN_IOCTL, &if_request) < 0) {
         cerr << "ERROR: trying to remove VLAN #" << vid << " to IF -:"
              << if_name << ":-  error: " << strerror(errno) << endl;
      }
      else {
         cout << "Removed VLAN with VID == " << vid << " from IF -:"
              << if_name << ":-" << endl;
      }
   }//if
   else if (strcasecmp(cmd, "set_dflt") == 0) {
      if (ioctl(fd, SET_DEFAULT_VLAN_ID_IOCTL, &if_request) < 0) {
         cerr << "ERROR: trying to set default VID #" << vid << " on IF -:"
              << if_name << ":-  error: " << strerror(errno) << endl;
      }
      else {
         cout << "Set default VID #" << vid << " on IF -:"
              << if_name << ":-" << endl;
      }
   }//if
   else if (strcasecmp(cmd, "add_port") == 0) {
      if (ioctl(fd, ADD_VLAN_TO_PORT_IOCTL, &if_request) < 0) {
         cerr << "ERROR: trying to add VLAN #" << vid << " to port -:"
              << if_name << ":-  error: " << strerror(errno) << endl;
      }
      else {
         cout << "Added VID #" << vid << " to port -:"
              << if_name << ":-" << endl;
      }
   }//if
   else if (strcasecmp(cmd, "rem_port") == 0) {
      if (ioctl(fd, REM_VLAN_FROM_PORT_IOCTL, &if_request) < 0) {
         cerr << "ERROR: trying to remove VLAN #" << vid << " from port -:"
              << if_name << ":-  error: " << strerror(errno) << endl;
      }
      else {
         cout << "Removed VID #" << vid << " from port -:"
              << if_name << ":-" << endl;
      }
   }//if
   else if (strcasecmp(cmd, "send") == 0) {
      if_request.u.skb_len = data_bytes_len;
      if_request.skb_data_type = data_type;
      memcpy(if_request.dest_mac, dest_mac, 6);
      memcpy(if_request.src_mac, src_mac, 6);
      memcpy(if_request.skb_data, data_bytes, data_bytes_len);

      if (ioctl(fd, HACK_SEND_PKT_IOCTL, &if_request) < 0) {
         cerr << "ERROR: sending packet to IF -:"
              << if_name << ":-  error: " << strerror(errno) << endl;
      }
      else {
         cout << "Sent packet to IF -:"
              << if_name << ":- data_type: " << data_type << endl;
      }
   }
   else {
      show_usage();
      exit(5);
   }

}/* main */