/* 
 * ether-forward: Forward ethernet broadcast packets to a specific 
 * interface
 * v0.3
 *
 * Used for this config:
 *
 *                 |router a               |  |router b               |
 *  192.168.0.10 -- 192.168.0.1 192.168.1.1 -- 192.168.1.2 192.168.0.2 -- 192.168.0.50
 *  192.168.0.15    eth0        wlan0          wlan0       eth0           192.168.0.60
 *
 *  Enabling of /proc/sys/net/ipv4/conf/<device>/proxy_arp and adding
 *  routes for every 0.x IP on the other side are needed 
 * (on router a: route add 192.168.0.50 gw 192.168.1.2)
 *
 *
 * Copyright (c) 2004 Sebastian Witt <se.witt@gmx.net>
 * All rights reserved.
 *
 * License: GPL v2
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``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 OR CONTRIBUTORS 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.
 * 
 */

#define _BSD_SOURCE

#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <net/ethernet.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <libnet.h>
#include <pcap.h>

static char errbuf[LIBNET_ERRBUF_SIZE];
static char *dev_eth;
static char *dev_wlan;
static u_char eth_broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
static u_char eth_addr[6];
static u_char wlan_addr[6];

void pcap_callback(u_char *args, const struct pcap_pkthdr *header, const u_char *packet)
{
	libnet_t *l;
	libnet_ptag_t eth = 0;
	char *ether_src = NULL;
	struct ip *iph;
	struct udphdr *udp;
	char *dev_out = NULL;
	
	// Select output device
	if(args[0] == 'e') dev_out = dev_wlan;
	else dev_out = dev_eth;
	
	// Init libnet
	l = libnet_init(LIBNET_LINK, dev_out, errbuf);
	
	if(l == NULL) {
		fprintf(stderr, "libnet_init() failed: %s\n", errbuf);
		goto end;
	}
	
	// Set src and dst ethernet address
	if(args[0] == 'e') {
		ether_src = wlan_addr;
	}
	else {
		ether_src = eth_addr;
	}

	// Get new packet
	iph = (struct ip *)(packet + ETHER_HDR_LEN);
	// UDP datagram?
	if(iph->ip_p == IPPROTO_UDP) {
		udp = (struct udphdr *)(packet + ETHER_HDR_LEN + iph->ip_hl*4);
		
		printf("UDP SRC: %s:%d->%d, dev_out: %s, len: %d\n", 
					inet_ntoa(iph->ip_src),
					ntohs(udp->uh_sport),
					ntohs(udp->uh_dport),
					dev_out,
					ntohs(iph->ip_len));
	}
	else {
		printf("Not a UDP broadcast packet from: %s\n", inet_ntoa(iph->ip_src));
	}
	
	// Build ethernet header
	eth = libnet_build_ethernet(
					eth_broadcast,	// dst addr.
					ether_src,		// src addr.
					ETHERTYPE_IP,
					(u_char *)iph,
					ntohs(iph->ip_len),
					l,
					0);

	if(eth == -1) {
		fprintf(stderr, "Failed to build ethernet header: %s\n",
						libnet_geterror(l));
		goto end;
	}

	// Write packet
	if(libnet_write(l) == -1) {
		fprintf(stderr, "Failed to write packet: %s\n", libnet_geterror(l));
		goto end;
	}
	
end:
	libnet_destroy(l);

}

int main(int argc, char *argv[])
{
	libnet_t *l;
	struct libnet_ether_addr *ether_addr;
	char pcap_error[PCAP_ERRBUF_SIZE];
	pcap_t *h_eth, *h_wlan;
	struct bpf_program p_filter_eth, p_filter_wlan;
	bpf_u_int32 pcap_netmask;
	bpf_u_int32 pcap_ip;
	char filter_eth[100];
	char filter_wlan[100];

	if(argc < 3) {
		fprintf(stderr, "Usage: %s eth_dev wlan_dev\n", argv[0]);
		return 1;
	}
	
	dev_eth = argv[1];
	dev_wlan = argv[2];

	// Get HW addresses
	if((l = libnet_init(LIBNET_LINK, dev_eth, errbuf)) == NULL) {
		fprintf(stderr, "libnet_init() failed: %s\n", errbuf);
		return 1;
	}
	ether_addr = libnet_get_hwaddr(l);
	memcpy(eth_addr, ether_addr->ether_addr_octet, 6);
	libnet_destroy(l);
	if((l = libnet_init(LIBNET_LINK, dev_wlan, errbuf)) == NULL) {
		fprintf(stderr, "libnet_init() failed: %s\n", errbuf);
		return 1;
	}
	ether_addr = libnet_get_hwaddr(l);
	memcpy(wlan_addr, ether_addr->ether_addr_octet, 6);
	libnet_destroy(l);


	// Open pcap devices
	h_eth = pcap_open_live(dev_eth, BUFSIZ, 0, 0, pcap_error);
	if(h_eth == NULL) {
		fprintf(stderr, "Failed to open pcap device: %s\n", pcap_error);
		return 1;
	}
	h_wlan = pcap_open_live(dev_wlan, BUFSIZ, 0, 0, pcap_error);
	if(h_wlan == NULL) {
		fprintf(stderr, "Failed to open pcap device: %s\n", pcap_error);
		return 1;
	}
	
	// Get IP and netmask of eth device
	if(pcap_lookupnet(dev_eth, &pcap_ip, &pcap_netmask, pcap_error) == -1) {
		fprintf(stderr, "Failed to run pcap_lookupnet(): %s\n", pcap_error);
		return 1;
	}
	
	// Build filter string for ethernet device
	sprintf(filter_eth, "ip and ether dst ff:ff:ff:ff:ff:ff and not ether src %02x:%02x:%02x:%02x:%02x:%02x",
					eth_addr[0], eth_addr[1],
					eth_addr[2], eth_addr[3],
					eth_addr[4], eth_addr[5]);
	printf("pcap filter for %s: %s\n", dev_eth, filter_eth);
	// Build filter for wlan device (all class c broadcast x.x.x.255)
	sprintf(filter_wlan, "ip[19] == 0xff and not ether src %02x:%02x:%02x:%02x:%02x:%02x",
					wlan_addr[0], wlan_addr[1],
					wlan_addr[2], wlan_addr[3],
					wlan_addr[4], wlan_addr[5]);
	printf("pcap filter for %s: %s\n", dev_wlan, filter_wlan);				
	
	// Compile filters
	if(pcap_compile(h_eth, &p_filter_eth, filter_eth, 0, pcap_netmask) == -1) {
		fprintf(stderr, "Failed to compile pcap ethernet filter\n");
		return 1;
	}
	if(pcap_compile(h_wlan, &p_filter_wlan, filter_wlan, 0, pcap_netmask) == -1) {
		fprintf(stderr, "Failed to compile pcap wlan filter\n");
		return 1;
	}
	
	// Set filter
	if(pcap_setfilter(h_eth, &p_filter_eth) == -1) {
		fprintf(stderr, "Failed to run pcap_setfilter()\n");
		return 1;
	}
	if(pcap_setfilter(h_wlan, &p_filter_wlan) == -1) {
		fprintf(stderr, "Failed to run pcap_setfilter()\n");
		return 1;
	}
	
	// Fork WLAN-loop
	if(!fork()) {
		printf("Running WLAN-loop\n");
		// WLAN-loop
		if(pcap_loop(h_wlan, -1, pcap_callback, "w") < 0) {
			fprintf(stderr, "Failed to run pcap_loop()\n");
			pcap_close(h_wlan);
			return 1;
		}
	}

	// Fork ETH-loop
	if(!fork()) {
		printf("Running ETH-loop\n");
		// Run pcap loop
		if(pcap_loop(h_eth, -1, pcap_callback, "e") < 0) {
			fprintf(stderr, "Failed to run pcap_loop()\n");
			pcap_close(h_eth);
			return 1;
		}
	}
	
	return 0;
}