Projects/Software/TunTapIO/pcap rsend.c

From Qontrol.nl Wiki
Jump to: navigation, search
/**********************************************************************
* rsfio - filtered raw socket to stdin/out proxy                      *
* Written by: Ivo Smits <Ivo@UFO-Net.nl>                              *
*                                                                     *
* Compile using: gcc pcap_rsend.c -o lpcsio -lpcap -pthread           *
*                                                                     *
* Many thanks to:                                                     *
*  - http://www.blug.linux.no/rfc1149/                                *
*  - http://linux.about.com/od/commands/l/blcmdl2_select.htm          *
*  - http://www.linuxjournal.com/article/4659                         *
*  - http://www.tcpdump.org/pcap.htm                                  *
*  - http://www.winpcap.org/docs/man/html/                            *
*  - http://www.tcpdump.org/lists/workers/2005/06/msg00102.html       *
*  - http://www.llnl.gov/computing/tutorials/pthreads/                *
*                                                                     *
**********************************************************************/

#include <pthread.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h> 
#ifndef HAVE_NETINET_IN_H
	#include <netinet/in.h>
#endif

#include <sys/ioctl.h>

#include <errno.h>
#include <dirent.h>

#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netpacket/packet.h>
#include <netinet/in.h>
#include <net/ethernet.h>
#include <net/if.h>
#include <linux/types.h>
#include <linux/filter.h>
#include <linux/if_ether.h>

#include <pcap.h>

pthread_t SendThread;
int SendSock;

int CaptureLen = ETH_FRAME_LEN;
int DumpStats = 1;
int IncludePLen = 1;

void got_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet);
void *SendLoop(void *arg);

// The main program, this is where all the magic happens
int main(int argc, char** argv) {
	//Parse command line arguments
	if (argc > 1) {
		if (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0) {
			fprintf(stderr, "rsio - raw socket to stdio proxy\n");
			fprintf(stderr, "Usage: %s INTF [CLEN] [FILTR]\n", argv[0]);
			fprintf(stderr, "  INTF:  the name of the network interface to connect to\n");
			fprintf(stderr, "  CLEN:  capture size (should be the same as the mtu, default: %d)\n", ETH_FRAME_LEN);
			fprintf(stderr, "  FILTR: tcpdump style filter expression (man tcpdump)\n");
			fprintf(stderr, "Note that the arguments should be in exactly THIS order.\n");
			fprintf(stderr, "Report bugs to <Ivo@UFO-Net.nl>\n");
			exit(0);
		}
	} else {
		//Application won't run without arguments
		//fprintf(stderr, "Try: %s -h\n", argv[0]);
		//exit(1);
	}

	//Different capture length?
	if (argc > 2) CaptureLen = atoi(argv[2]);
	if (CaptureLen < 1) {
		fprintf(stderr, "Capture length %d invalid!\n", CaptureLen);
		exit(1);
	}
	fprintf(stderr, "Max packet length: %d\n", CaptureLen);

	char *dev, errbuf[PCAP_ERRBUF_SIZE];

	if (argc > 1) {
		dev = argv[1];
	} else {
		if ((dev = pcap_lookupdev(errbuf)) == NULL) {
			fprintf(stderr, "Couldn't find default device: %s\n", errbuf);
			exit(1);
		}
	}
	fprintf(stderr, "Device: %s\n", dev);

	pcap_t *pcap;
	if ((pcap = pcap_open_live(dev, CaptureLen, 1, 0, errbuf)) == NULL) {
		fprintf(stderr, "Couldn't open device: %s\n", errbuf);
		exit(1);
	}

	if (argc > 3) {
		fprintf(stderr, "Filter: %s\n", argv[3]);
		struct bpf_program *bpfprog;
		if (pcap_compile(pcap, &bpfprog, argv[3], 0, 0) == -1) {
			fprintf(stderr, "Couldn't compile filter: %s\n", pcap_geterr(pcap));
			exit(1);
		}
		
		if (pcap_setfilter(pcap, &bpfprog) == -1) {
			fprintf(stderr, "Couldn't install filter: %s\n", pcap_geterr(pcap));
			exit(1);
		}
	}

	int Ret;

	if ((SendSock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) == -1) {
		fprintf(stderr, "Failed to create socket: %s\n", strerror(errno));
		exit(1);
	}
	//Bind to given interface by name
	{
		//Translate interface name to index
		struct ifreq ifr;
		memset(&ifr, 0, sizeof(ifr));
		strcpy(ifr.ifr_name, dev);
		if (ioctl(SendSock, SIOCGIFINDEX, &ifr) == -1) {
			fprintf(stderr, "Failed to get index for interface: '%s'\n", dev);
			close(SendSock);
			exit(1);
		}

		// Bind to given interface
		struct sockaddr_ll sll;
		memset(&sll, 0, sizeof(sll));
		sll.sll_family = AF_PACKET;
		sll.sll_ifindex = ifr.ifr_ifindex;
		if (bind(SendSock, (struct sockaddr *)&sll, (socklen_t)sizeof(sll)) == -1) {
			fprintf(stderr, "Could not bind to interface '%s': %s\n", dev, strerror(errno));
			close(SendSock);
			exit(1);
		}

		struct sock_filter BPF_code[]= {{ 0x6, 0, 0, 0x00000000 }};
		struct sock_fprog Filter; 

		Filter.len = 1;
		Filter.filter = BPF_code;

		/* Attach the filter to the socket */
		if (setsockopt(SendSock, SOL_SOCKET, SO_ATTACH_FILTER, &Filter, sizeof(Filter)) < 0) {
			fprintf(stderr, "Failed to attach filter...");
		}
	}

	if (Ret = pthread_create(&SendThread, NULL, SendLoop, NULL)) {
		fprintf(stderr, "Failed to create thread: %d\n", Ret);
		exit(1);
	}

	fprintf(stderr, "Starting capture!\n");
	pcap_loop(pcap, -1, got_packet, NULL);

	fprintf(stderr, "PCap finished: shutting down!\n");
	pcap_close(pcap);

	//pthread_kill(SendThread, SIG_KILL);
}

void got_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet) {
	if (DumpStats != 0) fprintf(stderr, "R: %d\n", header->caplen);
	if (IncludePLen == 1) write(1, &header->caplen, 4);
	write(1, packet, header->caplen);
}

void *SendLoop(void *arg) {
	unsigned char WBuf[CaptureLen]; //Data buffers
	int WBufLen = 0; //Packet length
	int PLen;

	//Start an infinite loop
	while (1) {
		//Read from stdin (FD 0)
		if (IncludePLen == 1) {
			WBufLen = read(0, &PLen, 4);
		}
		if (IncludePLen == 0 || PLen > CaptureLen) {
			PLen = CaptureLen;
		}
		if (IncludePLen == 0 || WBufLen > 0) {
			WBufLen = read(0, WBuf, PLen);
		}
		if (WBufLen == 0) {
			fprintf(stderr, "End of file on stdin\n");
			exit(0);
		} else if (WBufLen < 0) {
			fprintf(stderr, "Some error occured while reading stdin: %d\n", WBufLen);
			exit(1);
		}

		//Write to socket
		write(SendSock, WBuf, WBufLen);
		if (DumpStats != 0) fprintf(stderr, "W: %d\n", WBufLen);
	}
}