/*
 * Written 2001-09-23
 *
 * Copyright 2001 Vladislav V. Myasnyankin, Yekaterinburg Russia
 * hugevlad@yahoo.com http://cybervlad.port5.com
 * All rights reserved.
 * 
 * WARNING! Be very careful, because you can occasionaly crash
 * your network.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public License
 * as published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#include <stdio.h>
#include <unistd.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <netinet/if_ether.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

#define DEFAULT_DEVICE "eth0"
#define ETH_HW_ADDR_LEN 6

char usage[]={
"usage: stp \
[-v] \
[-dev <device>] \
[-dmac <dmac>] \
[-smac <smac>] \n\
-protoid <proto_id> \
-protovid <proto_v_id> \
-bpdu <bpdutype> \
-flags <flags> \n\
-rootid <rootid> \
-rootpc <rootpc> \
-brid <brid> \
-portid <portid> \
-mage <mage> \n\
-maxage <maxage> \
-htime <hellotime> \
-fdelay <fdelay> \n\
where:\n\
-v - be verbose and write output to file packet.dmp instead socket \n\
device - ethernet device name (default - eth0)\n\
dmac - destination MAC (default - 01:80:C2:00:00:00)\n\
smac - source MAC (default - MAC on given or default device)\n\
proto_id - Protocol Identifier (hex, 2 bytes)\n\
proto_v_id - Protocol Version Identifier (hex, 1 byte)\n\
bpdutype - BPDU type (hex, 1 byte)\n\
flags - flags value (hex, 1 byte)\n\
rootid - Root Identifier (hex, 8 bytes)\n\
rootpc - Root Path Cost (hex, 4 bytes)\n\
brid - Bridge Identifier (hex, 8 bytes)\n\
portid - Port Identifier (hex, 2 bytes)\n\
mage - Message Age (hex, 2 bytes)\n\
maxage - Max Age (hex, 2 bytes)\n\
hellotime - Hello Time (hex, 2 bytes)\n\
fdelay - Forward Delay (hex, 2 bytes)\n\
\n"};

struct bpdu_packet {
        u_char targ_hw_addr[ETH_HW_ADDR_LEN]; /* dest. ether address */
        u_char src_hw_addr[ETH_HW_ADDR_LEN];  /* src. ether address */
        u_char frame_type[2];                 /* 0x00 0x26 */
        u_char llc_dest;                      /* 0x42 */
        u_char llc_src;             /* 0x42 */
        u_char unknown;             /* 03 why? I don`t know :( */
        u_char proto_id[2];         /* Protocol Identifier */
        u_char proto_v_id;          /* Protocol Version Identifier */
        u_char bpdu_type;           /* BPDU Type */
        u_char flags;               /* Flags */
        u_char rootid[8];           /* Root Identifier */
        u_char rootpc[4];           /* Root Path Cost */
        u_char brid[8];             /* Bridge Identifier */
        u_char portid[2];           /* Port Identifier */
        u_char mage[2];             /* Message Age */
        u_char maxage[2];           /* Max Age */
        u_char hellotime[2]         /* Hello Time */
        u_char fdelay[2];           /* Forward Delay */
        u_char padding[8];          /* padding packet to 60 bytes */
};

void fireexit(char *); /* print error message and exit with code 1*/
void get_hex_value(char*,char*,int); /* convert string to hex */


/**** MAIN PART *****/
int main(int argc,char** argv){

FILE *f;
struct bpdu_packet pkt1;
struct sockaddr saddr;
struct ifreq ifr;
int sd,i;
char eth_dev[30];
unsigned char verbose=0;
unsigned int complete=0;
/*
array of bites:
14 device        7 rootid        0 fdelay
13 dmac          6 rootpc
12 smac          5 brid
11 proto_id      4 portid
10 proto_v_id    3 mage
9 bpdutype       2 maxage
8 flags          1 hellotime
*/


strncpy(eth_dev,DEFAULT_DEVICE,29);

for(i=1; i<argc; i++){
   if (!strncasecmp(argv[i],"-v",2)) verbose=1;
   if (!strncasecmp(argv[i],"-dev",4)){
      if (verbose) printf("Device: ");
      strncpy(eth_dev,argv[++i],29);
      if (verbose) printf("Ok\n");
      complete|=0x4000;
      };
   if (!strncasecmp(argv[i],"-dmac",5)){
      if (verbose) printf("Destination MAC: ");
      get_hex_value(pkt1.targ_hw_addr,argv[++i],6);
      if (verbose) printf("Ok\n");
      complete|=0x2000;
      };
   if (!strncasecmp(argv[i],"-smac",5)){
      if (verbose) printf("Source MAC: ");
      get_hex_value(pkt1.src_hw_addr,argv[++i],6);
      if (verbose) printf("Ok\n");
      complete|=0x1000;
      };
   if (!strncasecmp(argv[i],"-protoid",8)){
      if (verbose) printf("Protocol Identifier: ");
      get_hex_value(pkt1.proto_id,argv[++i],2);
      if (verbose) printf("Ok\n");
      complete|=0x0800;
      };
   if (!strncasecmp(argv[i],"-protovid",9)){
      if (verbose) printf("Protocol Version Identifier: ");
      get_hex_value(&pkt1.proto_v_id,argv[++i],1);
      if (verbose) printf("Ok\n");
      complete|=0x0400;
      };
   if (!strncasecmp(argv[i],"-bpdu",5)){
      if (verbose) printf("BPDU Type: ");
      get_hex_value(&pkt1.bpdu_type,argv[++i],1);
      if (verbose) printf("Ok\n");
      complete|=0x0200;
      };
   if (!strncasecmp(argv[i],"-flags",6)){
      if (verbose) printf("Flags: ");
      get_hex_value(&pkt1.flags,argv[++i],1);
      if (verbose) printf("Ok\n");
      complete|=0x0100;
      };
   if (!strncasecmp(argv[i],"-rootid",7)){
      if (verbose) printf("Root Identifier: ");
      get_hex_value(pkt1.rootid,argv[++i],8);
      if (verbose) printf("Ok\n");
      complete|=0x0080;
      };
   if (!strncasecmp(argv[i],"-rootpc",7)){
      if (verbose) printf("Root Path Cost: ");
      get_hex_value(pkt1.rootpc,argv[++i],4);
      if (verbose) printf("Ok\n");
      complete|=0x0040;
      };
   if (!strncasecmp(argv[i],"-brid",5)){
      if (verbose) printf("Bridge Identifier: ");
      get_hex_value(pkt1.brid,argv[++i],8);
      if (verbose) printf("Ok\n");
      complete|=0x0020;
      };
   if (!strncasecmp(argv[i],"-portid",7)){
      if (verbose) printf("Port Identifier: ");
      get_hex_value(pkt1.portid,argv[++i],2);
      if (verbose) printf("Ok\n");
      complete|=0x0010;
      };
   if (!strncasecmp(argv[i],"-mage",5)){
      if (verbose) printf("Message Age: ");
      get_hex_value(pkt1.mage,argv[++i],2);
      if (verbose) printf("Ok\n");
      complete|=0x0008;
      };
   if (!strncasecmp(argv[i],"-maxage",7)){
      if (verbose) printf("Max Age: ");
      get_hex_value(pkt1.maxage,argv[++i],2);
      if (verbose) printf("Ok\n");
      complete|=0x0004;
      };
   if (!strncasecmp(argv[i],"-htime",6)){
      if (verbose) printf("Hello Time: ");
      get_hex_value(pkt1.hellotime,argv[++i],2);
      if (verbose) printf("Ok\n");
      complete|=0x0002;
      };
   if (!strncasecmp(argv[i],"-fdelay",7)){
      if (verbose) printf("Forward delay: ");
      get_hex_value(pkt1.fdelay,argv[++i],2);
      if (verbose) printf("Ok\n");
      complete|=0x0001;
      };
};

/* Check, if all needed parameters set */
if ((complete & 0x0FFF) < 0x0FFF) fireexit(usage);

/* Set constant values */
pkt1.frame_type[0]=0x00;
pkt1.frame_type[1]=0x26;
pkt1.llc_dest=0x42;
pkt1.llc_src=0x42;
pkt1.unknown=03;
bzero(pkt1.padding,8);

if (verbose) {
    f=fopen("packet.dmp","a");
    fwrite(&pkt1,1,sizeof(pkt1),f);
    fclose(f);
    exit(0);
    };

/* Open raw socket */
if ((sd = socket(AF_INET, SOCK_PACKET, htons(ETH_P_ALL))) < 0){
        perror("Can't get raw socket: ");
        exit(1);
        }

/* Get our hardware address, if not given */
if (!(complete & (12<<1))){
    strcpy(ifr.ifr_name, eth_dev);
    if (ioctl(sd, SIOCGIFHWADDR, &ifr) < 0){
	 perror("Can't get hardware address: ");
         close(sd);
         exit(1);
        };
    memcpy(pkt1.src_hw_addr,ifr.ifr_hwaddr.sa_data,6);
    };

/* Set device to use */
strcpy(saddr.sa_data,eth_dev);

/* Send prepared packet */
if(sendto(sd,&pkt1,sizeof(pkt1),0,&saddr,sizeof(saddr)) < 0)
   perror("Send packet");

/* Close socket */
close(sd);

exit(0);
}

/*************** END MAIN PART ********************/

void fireexit(char* str){
fprintf(stderr,"%s\n",str);
exit(1);
}

void get_hex_value(char* buf,char* str,int len){

int i;
char c,val;

for(i=0;i<len;i++){
        if( !(c = tolower(*str++))) fireexit("Invalid hex value");
        if(isdigit(c)) val = c-'0';
        else if(c >= 'a' && c <= 'f') val = c-'a'+10;
        else fireexit("Invalid hex value");

        *buf = val << 4;
        if( !(c = tolower(*str++))) fireexit("Invalid hex value");
        if(isdigit(c)) val = c-'0';
        else if(c >= 'a' && c <= 'f') val = c-'a'+10;
        else fireexit("Invalid hex value");

        *buf++ |= val;

        if(*str == ':')str++;
        }
}