softswitch.c

在Linux操作系统下实现的软件交换技术

#include <pcap.h>
#include <libnet.h>
#include <pthread.h>

#include <stdio.h>
#include "ethernet.h"
#include "getmac_struct.h"
#define  P printf

#define MAX_NICS 6
#define MAX_THREADS 6;

const struct ethernet *ethernet; /* The ethernet header */
struct forwarding_table table;

struct queue queue_port[8];

char *eth[]= {"eth0","eth1","eth2","eth3","eth4","eth5"};

pthread_mutex_t mutex;//khoi tao bien mutex toan cuc

//==========================================================
//Create buffer queueing
//==========================================================
	void init_queue(int m)
	{
		queue_port[m].head = NULL;
		queue_port[m].tail = NULL;
	}

//==========================================================
//Add packets into buffer queue
//==========================================================
	void add_packet(const u_char *packet,struct pcap_pkthdr *header,int m)
	{
	struct buffer_queue *p;
	const u_char *pket;

		if(queue_port[m].head == NULL)
		{
		queue_port[m].head = malloc(sizeof(struct buffer_queue));
		queue_port[m].head->pkt.packet=packet;
		memcpy(&queue_port[m].head->pkt.header,header,sizeof(struct pcap_pkthdr));
		queue_port[m].head->next=NULL;
		//head->num_Packet=1;
		queue_port[m].tail=queue_port[m].head;
		}
		else
		{
		p= malloc(sizeof(struct buffer_queue));
		//head->num_Packet++;
		p->pkt.packet=packet;
		memcpy(&queue_port[m].head->pkt.header,header,sizeof(struct pcap_pkthdr));
		p->next=NULL;
		queue_port[m].tail->next=p;
		queue_port[m].tail=p;
		//head->num_Packet++;
		}//end of head=null
	pket=queue_port[m].tail->pkt.packet;

	ethernet = (struct ethernet *) pket;

	//P("MAC_src :%02x:%02x:%02x:%02x:%02x:%02x ->-> ",ethernet->ether_src[0],ethernet->ether_src[1],ethernet->ether_src[2],ethernet->ether_src[3],ethernet->ether_src[4],ethernet->ether_src[5]);
	//P("MAC_des :%02x:%02x:%02x:%02x:%02x:%02x\n",ethernet->ether_des[0],ethernet->ether_des[1],ethernet->ether_des[2],ethernet->ether_des[3],ethernet->ether_des[4],ethernet->ether_des[5]);

	}

//======================================================================
//get packets from buffer queue
//======================================================================
	struct packet* get_packet(int m)
	{
	struct buffer_queue *p;
	struct packet *pket;

	if(queue_port[m].head!=NULL)
	{
		pket=&queue_port[m].head->pkt;
		p=queue_port[m].head;
		queue_port[m].head=queue_port[m].head->next;
		//free(p);
		ethernet = (struct ethernet *) pket->packet;

		P("MAC_src :%02x:%02x:%02x:%02x:%02x:%02x ->-> ",ethernet->ether_src[0],ethernet->ether_src[1],ethernet->ether_src[2],ethernet->ether_src[3],ethernet->ether_src[4],ethernet->ether_src[5]);
		P("MAC_des :%02x:%02x:%02x:%02x:%02x:%02x\n",ethernet->ether_des[0],ethernet->ether_des[1],ethernet->ether_des[2],ethernet->ether_des[3],ethernet->ether_des[4],ethernet->ether_des[5]);

		return pket;

	}

	return NULL;
	}

//======================================================================
//free buffer queue after use
//======================================================================

void free_queue(int m)
{

	struct buffer_queue *p;

	while(queue_port[m].head->next!=NULL)
	{
	p=queue_port[m].head;
	queue_port[m].head=queue_port[m].head->next;
	free(p);
	}

	free(queue_port[m].head);
}

//======================================================================
//Print forwarding table (if you want to see it)
//======================================================================
void print_table(struct forwarding_table *table)
{
  	int i,j,n;
	n=table->count_Mac;

	printf("Forwarding table:\n");

	for(i=0;i<n;i++)
	{
		 for (j=0; j<6; ++j)
		{
			printf("%2.2x:", table->Mac_Host[i][j]);
		}
		P(" | eth%x\n", table->eth[i]);
	 }
}

//======================================================================
//function forward packets
//======================================================================
void send_packet(int e)
{
	char *dev;
	libnet_t *l;

	char errbuf[LIBNET_ERRBUF_SIZE];

	int r;
	u_char *buffer;
	struct packet *packet;



	switch (e)
	{
	case 0: dev = "eth0";
		break;
	case 1: dev = "eth1";
		break;
	case 2: dev = "eth2";
		break;
	case 3: dev = "eth3";
		break;
	default: dev = "eth4";

	}

		//P(" Send to %s\n",dev);




	 l = libnet_init(
            LIBNET_LINK_ADV,                           //  injection type
            dev,                                 //network interface
            errbuf);                                // errbuf


	if (l == NULL)
    	{
        	fprintf(stderr, "libnet_init() failed: %s", errbuf);
        	exit(EXIT_FAILURE);
    	}



	while(1)
	{

	pthread_mutex_lock(&mutex);
	packet=(struct packet*)get_packet(e);

	//P(" Send to %s\n",dev);

	if(packet!=NULL)
	{

	//buffer=(u_char*)malloc(packet->header.len);

		//if(buffer!=NULL)
		//{
		//memcpy(buffer,packet->packet,packet->header.len);

		libnet_adv_write_link(l, (u_char*)packet->packet, packet->header.len);

		//ethernet = (struct ethernet *) buffer;
		ethernet = (struct ethernet *) packet->packet;

		P(" Send to %s\n",dev);
		P(" MAC_src :%02x:%02x:%02x:%02x:%02x:%02x  ->-> ",ethernet->ether_src[0],ethernet->ether_src[1],ethernet->ether_src[2],ethernet->ether_src[3],ethernet->ether_src[4],ethernet->ether_src[5]);
		P(" MAC_des :%02x:%02x:%02x:%02x:%02x:%02x\n",ethernet->ether_des[0],ethernet->ether_des[1],ethernet->ether_des[2],ethernet->ether_des[3],ethernet->ether_des[4],ethernet->ether_des[5]);

		//free(buffer);
		//}
 	}//end of packet!=NULL

	pthread_mutex_unlock(&mutex);

	}//end of while(1)

libnet_destroy(l);

}

//======================================================================
//Function capture packets
//======================================================================
    void capture_packet(int n)
    {
	pcap_t* handle;				//Define session handle
	char *dev;

	char errbuf[PCAP_ERRBUF_SIZE];          // Error string
	int i,j,k;
	int flag;

	int num_nics;				//number of NICs
	const u_char *packet;

	bpf_u_int32 mask;                       // Our netmask
        bpf_u_int32 net;                        // Our IP
        struct pcap_pkthdr header;              // The header that pcap gives us

	num_nics = count_nics();

	switch (n)
	{
	case 0: dev = "eth0";
		break;
	case 1: dev = "eth1";
		break;
	case 2: dev = "eth2";
		break;
	case 3: dev = "eth3";
		break;
	default: dev = "eth4";

	}

	/* ask pcap for the network address and mask of the device */
	pcap_lookupnet(dev, &net, &mask, errbuf);
        /* Open the session in promiscuous mode */
	//param 4, if=0 ->sniff until error occur, =-1 ->sniff indefinitely
        handle = pcap_open_live(dev, BUFSIZ, 1, -1, errbuf);
	if(handle == NULL)
		{ printf("pcap_open_live(): %s\n",errbuf); exit(1); }

	pcap_direction(handle, D_IN); 	//just capture incoming packets
	//pcap_direction(handle, D_OUT);		//just capture outgoing //packets
	pcap_direction(handle, D_INOUT);	//capture both

	while(1)
	{

	packet = pcap_next(handle, &header);	//capture packet by using pcap_next()

	if(packet!=NULL)
	{
	//P("Packet len is: %d\n",header.len); 	//print packet len

	ethernet = (struct ethernet *) packet;	//typecast packet

	//Update to forwarding table
	if(table.count_Mac<4)
	{
		for(i=0; i<4; i++ )
		{
			flag =1;

			for(j=0; j<6; j++ )
			if(ethernet->ether_src[j] != table.Mac_Host[i][j])
			{
				flag = 0;
				break;
			}

			if(flag == 1)
			{
				break;
			}
		}//end of for

			if(flag == 0)
			{

				for(j=0; j<6; j++ )
				table.Mac_Host[table.count_Mac][j]=ethernet->ether_src[j];

				table.eth[table.count_Mac]=n;
				table.count_Mac++;

				P("\n");
				print_table(&table);	//print forwarding table if needed
				P("\n");
			}

	}//end of if table.count_Mac<4
	//update to forwarding table finished here

	//add_packet(packet,&header);	//add packet into buffer queue

 //Now we add packets into proper queue
	for(i=0; i<table.count_Mac; i++ )
	{
   		flag =1;

		for(j=0; j<6; j++ )
		if(ethernet->ether_des[j] != table.Mac_Host[i][j])
		{
			flag = 0;
			break;
		}

		if(flag == 1)	//Mac des exist in forwarding table
		{
			//P("Capture from %s,send to eth %d", dev,i);
			//send_packet(&header, packet, i);	//Send to exact port
			pthread_mutex_lock(&mutex);
			add_packet(packet,&header,i);
			pthread_mutex_unlock(&mutex);
			break;
		}
	}//end of for

	if(flag==0)	//Mac des does not exist in forwarding table
	{
		//add packet to all queue_port except queue_port(n)
		for(i=0; i<num_nics; i++ )
		{
			if(i!=n)
			{
				//P("Capture from %s,send to eth %d", dev,i);
				//send_packet(&header, packet,i);
				pthread_mutex_lock(&mutex);
				add_packet(packet,&header,i);
				pthread_mutex_unlock(&mutex);
				//sleep(1);
			}
		}
	}

	}//end of if packet!=NULL

	}//end of while

	pcap_close(handle);
    }

//======================================================================
//function execute threads
//======================================================================
	void *do_thread_recv(void *data)
	{
		int thr_id = (int)data;
		switch (thr_id)
		{
		case 0:
			capture_packet(0);
			break;
		case 1:
			capture_packet(1);
			break;
		case 2:
			capture_packet(2);
			break;
		case 3:
			capture_packet(3);
			break;

		default:capture_packet(4);

		}
	}

//======================================================================
//do thread send
//======================================================================
void *do_thread_send(void *data)
	{
		int thr_id = (int)data;
		switch (thr_id)
		{
		case 0:
			send_packet(0);
			break;
		case 1:
			send_packet(1);
			break;
		case 2:
			send_packet(2);
			break;
		case 3:
			send_packet(3);
			break;

		default:send_packet(4);

		}
	}

//======================================================================
//Main start here
//======================================================================
    int main()
    {

	pthread_t thread_recv[MAX_NICS];
	int thr_id_recv[MAX_NICS];

	pthread_t thread_send[MAX_NICS];
	int thr_id_send[MAX_NICS];


	long stat;
	int i;
  	u_char addr[6];

	int num_nics = count_nics();		//count number of NIC on this Switch

	int a[num_nics];

	table.count_Mac=0;			//initialize count_Mac of forwarding table

	system("clear");			//Clear screen.
	P("Checking network card(s)......\n");
	P("Found %d NIC card on this switch.\n\n",num_nics);

	for (i =0;i<num_nics;i++)
    	{
    	a[i]=i;
    	stat = get_mac( addr,a[i]);
    		if (0 == stat)
    		{
			print_mac(addr,a[i]);
		}
		else
		{
		fprintf( stderr, "Can't get MAC address\n");
        	exit( 1);
    		}
	}
	P("\nPlease wait until i found a packet on the wire...\n");

 	//=============================================
	//khoi tao mutex
	pthread_mutex_init(&mutex,NULL);

		i =0;
		do
		{
		init_queue(i);
		sleep(1);
		thr_id_recv[i] = i;
		thr_id_send[i] = i;
	 	//create threads
		pthread_create(&thread_recv[i], NULL, do_thread_recv, (void*)thr_id_recv[i]);
		pthread_create(&thread_send[i], NULL, do_thread_send, (void*)thr_id_send[i]);
		i++;
		}while(i<num_nics);

	while(1)
	{
		sleep(1);
	}

	for(i=0;i<num_nics;i++)
	free_queue(i);	//free buffer queue after useing

	return 0;
    }