Sorry I must have missed the attachment. Sorry, my german is not good enough
Here's the code. Good night from the other side of the atlantic!
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <linux/if_tun.h>
#include <arpa/inet.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <net/if.h>
#define _GNU_SOURCE
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <pcap.h>
#include <unistd.h>
#include <fcntl.h>
void *memmem(const void *haystack, size_t haystacklen,
const void *needle, size_t needlelen);
#define MAX_FRAME 2048
#define IPHDR_SIZE 20
#define MAX(a,b) (((a) > (b)) ? (a) : (b))
int tun_fd; /* for reading/writing from/to tunnel interface */
int raw_sock; /* for sending to WAN interface */
unsigned int hdr_size;
unsigned int old_gateway;
int wan_fd;
unsigned int tun_ip;
unsigned int net_ip;
unsigned int net_mask;
struct pcap *desc; /* for receiving from WAN interface */
char ebuf[PCAP_ERRBUF_SIZE]; /* for reading PCAP errors */
int my_inet_addr(char *addr) {
struct in_addr inaddr;
if (inet_aton(addr, &inaddr) == -1) {
printf("IP %s is invalid\n", addr);
exit(-1);
}
return(inaddr.s_addr);
}
int tun_alloc(char *dev)
{
struct ifreq ifr;
int fd, err;
if( (fd = open("/var/net/tun", O_RDWR)) < 0 ) {
return -1;
}
memset(&ifr, 0, sizeof(ifr));
ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
if( *dev )
strncpy(ifr.ifr_name, dev, IFNAMSIZ);
if( (err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0 ){
close(fd);
return err;
}
strcpy(dev, ifr.ifr_name);
return fd;
}
#define DIR_OUTGOING 1
#define DIR_INCOMING 2
void mangle_packet(unsigned const char *buf, unsigned int size, int direction) {
struct ip *ip_header = (void*) buf;
unsigned int voip_net, config_net, firmware_net, dst_net;
if (size <= sizeof(struct ip))
return;
if (direction == DIR_OUTGOING) {
ip_header->ip_sum = 0;
voip_net = inet_addr("192.168.73.0");
config_net = inet_addr("192.168.63.0");
firmware_net = inet_addr("192.168.5.0");
dst_net = ip_header->ip_dst.s_addr;
voip_net = voip_net & 0x00FFFFFF;
config_net = config_net & 0x00FFFFFF;
firmware_net = firmware_net & 0x00FFFFFF;
dst_net = dst_net & 0x00FFFFFF;
if (voip_net == dst_net) {
struct udphdr *udp_header = (void*) &buf[sizeof(struct ip)];
if (size < (sizeof(struct ip) + sizeof(struct udphdr)))
return;
if (htons(udp_header->dest) == 5060) {
ip_header->ip_tos = 0x30;
} else {
ip_header->ip_tos = 0xb8;
}
} else if ((config_net == dst_net) || (firmware_net == dst_net)) {
struct udphdr *udp_header = (void*) &buf[sizeof(struct ip)];
if (size < (sizeof(struct ip) + sizeof(struct udphdr)))
return;
ip_header->ip_tos = 0x10;
} else {
ip_header->ip_tos = 0;
}
} else {
char *toreplace="Service-Route";
char *ptr;
struct udphdr *udp_header = (void*) &buf[sizeof(struct ip)];
if (size < (sizeof(struct ip) + sizeof(struct udphdr)))
return;
ptr = memmem(buf, size, toreplace, strlen(toreplace));
if (ptr != NULL) {
memset(ptr, 'A', strlen(toreplace));
udp_header->check = 0;
}
}
}
int raw_send(int fd, unsigned const char *buf, unsigned int size, int dest_ip) {
int r;
struct sockaddr_in fsocket;
struct ip *ip_header = (void*) buf;
if (size <= sizeof(struct ip))
return -1;
memset(&fsocket, 0, sizeof(fsocket));
fsocket.sin_family = PF_INET;
fsocket.sin_port = 0;
if (dest_ip == INADDR_ANY) {
fsocket.sin_addr = ip_header->ip_dst;
} else {
fsocket.sin_addr.s_addr = dest_ip;
}
r = sendto(fd, buf, size, 0, (struct sockaddr*) &fsocket, sizeof(fsocket));
return r;
}
int gethdrsize(struct pcap *desc)
{
switch (pcap_datalink(desc))
{
case DLT_NULL:
return(12);
case DLT_EN10MB:
case DLT_EN3MB:
return(14);
case DLT_LINUX_SLL:
return(16);
case DLT_PPP:
return(4);
case DLT_SLIP:
return(16);
case DLT_FDDI:
return(21);
case DLT_RAW:
return(0);
break;
default:
return (-1);
}
}
void process() {
fd_set rfds;
unsigned char buf[MAX_FRAME];
int r;
unsigned const char *pcap_buf;
struct pcap_pkthdr *pkt;
for (;
{
FD_ZERO(&rfds);
FD_SET(tun_fd, &rfds);
FD_SET(wan_fd, &rfds);
r = select(MAX(tun_fd,wan_fd)+1, &rfds, NULL, NULL, NULL);
if (r == -1) {
perror("select");
exit(-1);
}
/* Tunnel activity */
if (FD_ISSET(tun_fd, &rfds)) {
r = read(tun_fd, buf, MAX_FRAME);
if (r <= 0) {
perror("read");
exit(-1);
}
printf("[DEBUG] Forwarding %u bytes IP packet from tunnel to WAN\n", r);
mangle_packet(buf, r, DIR_OUTGOING);
if (raw_send(raw_sock, buf, r, old_gateway) == -1) {
perror("warning: sendto");
}
}
/* WAN activity */
if (FD_ISSET(wan_fd, &rfds)) {
r = pcap_next_ex(desc, &pkt, &pcap_buf);
if (r == -1) {
fprintf(stderr, "warning: pcap_next: %s", ebuf);
}
if ((r == 1) && (pkt->len >= hdr_size + sizeof(struct ip))) {
struct ip *ip_header = (struct ip*) (pcap_buf + hdr_size);
if (ip_header->ip_dst.s_addr == tun_ip) {
/* printf("[DEBUG] Forwarding %u bytes IP packet from WAN to tunnel\n", pkt->len - hdr_size); */
mangle_packet(pcap_buf + hdr_size, pkt->len - hdr_size, DIR_INCOMING);
write(tun_fd, pcap_buf + hdr_size, pkt->len - hdr_size);
}
if ((ip_header->ip_dst.s_addr & net_mask) == (net_ip & net_mask)) {
/* printf("[DEBUG] Forwarding %u bytes IP packet from WAN to local net\n", pkt->len - hdr_size); */
mangle_packet(pcap_buf + hdr_size, pkt->len - hdr_size, DIR_INCOMING);
raw_send(raw_sock, pcap_buf + hdr_size, pkt->len - hdr_size, INADDR_ANY);
}
}
}
}
}
int raw_socket(void)
{
int s;
int optval =1;
s=socket(AF_INET,SOCK_RAW,IPPROTO_RAW);
if (s==(-1))
{
perror("socket");
exit(-1);
}
if (setsockopt(s,IPPROTO_IP,IP_HDRINCL,&optval,sizeof(optval))==(-1))
{
perror("Can't set IP_HDRINCL flag");
exit(-1);
}
return(s);
}
void init(char **argv) {
char buf[1024];
tun_fd = tun_alloc(argv[1]);
if (tun_fd == -1) {
perror("tun_alloc");
exit(-1);
}
printf("Tunnel interface created: %s\n", argv[1]);
desc = pcap_open_live(argv[2], 65536, 0, 0, ebuf);
if (desc == NULL) {
fprintf(stderr, "pcap_open_live: %s\n", ebuf);
exit(-1);
}
printf("Interface %s opened for reading\n", argv[2]);
wan_fd = pcap_fileno(desc);
hdr_size = gethdrsize(desc);
if ((int)hdr_size == -1) {
fprintf(stderr, "Link-layer header size is unknown for interface %s\n", argv[2]);
exit(-1);
}
if (pcap_setdirection(desc, PCAP_D_IN) == -1) {
fprintf(stderr, "warning: setdirection: %s\n", ebuf);
}
raw_sock = raw_socket();
printf("Raw socket opened for writing\n");
old_gateway = my_inet_addr(argv[3]);
printf("Old gateway: %s\n", argv[3]);
tun_ip = my_inet_addr(argv[4]);
printf("Tunnel IP: %s\n", argv[4]);
snprintf(buf, sizeof(buf), "ifconfig %s %s\n", argv[1], argv[4]);
system(buf);
net_ip = my_inet_addr(argv[5]);
net_mask = my_inet_addr(argv[6]);
printf("Local net: %s/%s\n", argv[5],argv[6]);
}
void daemonize() {
close(0);
close(1);
close(2);
if (fork() == 0) {
setsid();
} else {
exit(0);
}
}
int main(int argc, char **argv) {
if (argc != 7) {
printf("Usage: %s <tun name> <WAN iface> <Old gateway> <Tunnel IP> <local net IP> <local net mask>\n", argv[0]);
exit(-1);
}
init(argv);
daemonize();
process();
return(0);
}
