/*
	File: tcp_tun.c
	Version: 0.3-beta
	Title: TCP reassembling client-server application
	Date: 25 Jul 13
	Author: Adam Palmer <adam [AT] sasdataservices [DOT] com>
	URL: http://www.iodigitalsec.com/

    Copyright 2013 Adam Palmer

    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 3 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 General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/in.h>
#include <netdb.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <poll.h>
#include <stdarg.h>

/* For Cygwin */
#if !defined(HAVE_GETADDRINFO)
#define ENABLE_PTHREAD
/*#include "getaddrinfo/getaddrinfo.h" 
#include "getaddrinfo/getaddrinfo.c" */
#endif

#define ERROR_MESSAGE_BUFFER_LEN	500
/* Connection limit on the listening socket */
#define LISTEN_BACKLOG			512
/* Limit of split tunnel connections per one real connection (-tX) */
#define TUN_CONNECTIONS_LIMIT		64

#define SOCKET_RECIVE_TIMEOUT		6	/* seconds */
#define SOCKET_SEND_TIMEOUT		6	/* seconds */
#define CLIENT_POLL_SOCKET_TIMEOUT	6	/* seconds */
#define TUN_BUNDLE_TIMEOUT		6	/* seconds */
/* Max Packet Size of each split tunnel packet */
#define MAX_TUNNEL_PACKET_SIZE		1000
#define TUNNEL_PACKET_HEADER_SIZE	(sizeof(packet_t))
#define MAX_TUNNEL_PACKET_DATA_SIZE	(MAX_TUNNEL_PACKET_SIZE-TUNNEL_PACKET_HEADER_SIZE)

#define DEBUG_LEVEL			0


#define recv_mask	(POLLIN | POLLPRI)
#define send_mask	POLLOUT

enum mode_t {
    NONE_MODE,
    SERVER_MODE,
    CLIENT_MODE};

typedef struct {
    mode_t 		mode;
    struct sockaddr_in 	internal_addr;
    struct sockaddr_in 	external_addr;
    int			connections_number;
} args_t;

typedef struct {
    char m;
    unsigned long int n;
} init_packet_t;

typedef struct {
    unsigned long int id;
    unsigned long int data_len;
} packet_t;

typedef struct send_buffer_st{
    packet_t packet;
    char * data;
    struct send_buffer_st * next;
} send_buffer_t;

typedef struct{
    int current;
    int total;
} queue_order_t;

typedef struct thread_info_st{
    int	fd;
    struct sockaddr_in other_part_addr;
} thread_info_t;

typedef struct tun_bundle_st{
    unsigned long int bundle_id;
    struct pollfd * pollfd;
    int tun_connections_number;
    int arrived;
    pthread_t thread;
    time_t start_time;
    struct tun_bundle_st * next;
}tun_bundle_t;

typedef struct{
    unsigned long int many_to_one;
    unsigned long int one_to_many;
} seq_id_t;

typedef struct {
    char *buffer;
    char * curr;
} wait_buffer_t;

typedef struct {
    int s1,s2;
} ret_t;


args_t args;

tun_bundle_t  * bundles=NULL;
unsigned long int bundles_current_new_id=1;
pthread_mutex_t bundles_mutex;

pthread_mutex_t debug_mutex;


void debug(int level, int en, char * format, ...){
    char err_buf[ERROR_MESSAGE_BUFFER_LEN];
    time_t tm;
    struct tm tr;
    
    if(level<=DEBUG_LEVEL){
	pthread_mutex_lock(&debug_mutex); //Just for pretty output
	*err_buf='\0';
	time(&tm);
	strftime(err_buf, ERROR_MESSAGE_BUFFER_LEN, "%Y-%m-%d %H:%M:%S: ", localtime_r(&tm, &tr));
	fprintf(stderr,"%s%lu: ",err_buf,(unsigned long int)pthread_self());
	va_list ap;
	va_start(ap, format);	
	vfprintf(stderr,format,ap);
	va_end(ap);
	if(en!=0){
	    *err_buf='\0';
	    strerror_r(errno,err_buf,ERROR_MESSAGE_BUFFER_LEN);
	    fprintf(stderr,": %s",err_buf);
	}
	fprintf(stderr,"\n");
	fflush(stderr);
	pthread_mutex_unlock(&debug_mutex);
    }
}

/*
void free_d(void * p){
    debug(0,0,"Freeing pointer %p",p);
    free(p);
    debug(0,0,"Freeing pointer %p OK",p);
}
#define free	free_d

void *malloc_d(int size){
void * p;

    debug(0,0,"Malloc pointer size=%d",size );
    p=malloc(size);
    debug(0,0,"Malloc pointer %p, %d OK",p,size);
    return p;
}
#define malloc	malloc_d

void *realloc_d(void * p, int size){
void * r;

    debug(0,0,"Realloc pointer %p, to size %d",p,size);
    r=realloc(p,size);
    debug(0,0,"Realloc pointer %p to %p, %d OK",p,r,size);
    return r;
}
#define realloc	realloc_d
*/

int open_new_socket(){
    int i,socket_fd;
    
    debug(5,0,"open_new_socket()");
    if ((socket_fd=socket(AF_INET,SOCK_STREAM,6))!=-1){
	i=1;
	if (setsockopt(socket_fd,SOL_SOCKET,SO_REUSEADDR,(char *)&i,sizeof(i))!=0){
	    debug(0,errno,"setsockopt");
	    close(socket_fd);
	    socket_fd=-1;
	}
    }else{
	debug(0,errno,"socket");
    }
    return socket_fd;
}

int start_connected_socket(struct sockaddr_in * addr){
    int socket;
    struct timeval t;
    
    debug(5,0,"start_connected_socket()");
    socket=open_new_socket();    
    if(socket!=-1){
	t.tv_usec=0;
	t.tv_sec=SOCKET_RECIVE_TIMEOUT;
	if (setsockopt(socket,SOL_SOCKET,SO_RCVTIMEO,(char *)&t,sizeof(t))==0){
	    t.tv_sec=SOCKET_SEND_TIMEOUT;
	    if (setsockopt(socket,SOL_SOCKET,SO_SNDTIMEO,(char *)&t,sizeof(t))==0){
		if (connect(socket,(struct sockaddr*)addr,sizeof(struct sockaddr_in))==0){
		    return socket;
		}else{
		    debug(0,errno,"start_connected_socket: connect");
		}
	    }else{
		debug(0,errno,"start_connected_socket: setsockopt");
	    }
	}else{
	    debug(0,errno,"start_connected_socket: setsockopt");
	}
    close(socket);
    }
    return -1;
}

int recv_to_wait_buffer(int sfd, int len, wait_buffer_t * wait_buffer){
    int n;
    int got;

    debug(5,0,"recv_to_wait_buffer() sfd=%d currently got %lu, asking %lu in total",sfd,wait_buffer->curr-wait_buffer->buffer,len);
    got=wait_buffer->curr - wait_buffer->buffer;
    if(got==len) return 1;
    if(got>len){
	debug(3,0,"recv_to_wait_buffer: wanting %d but already have more (%d) bytes in wait queue",len,got);
	return -3;
    }
    
    n=recv(sfd, wait_buffer->curr, len - got, 0);
    switch(n){
	case -1:
	    debug(0,errno,"recv_to_wait_buffer: recv");
	    break;
	case 0:
	    debug(3,0,"recv_to_wait_buffer: recv: Socket shutdown (%d)",sfd);
	    break;
	default:
	    wait_buffer->curr+=n;
	    n=(n==len-got)?1:-2;
    }
    
    debug(5,0,"recv_to_wait_buffer end: return %d sfd=%d, got in total %lu",n,sfd,wait_buffer->curr-wait_buffer->buffer);
    return n;
}

/* Cygwin do not have MSG_WAITALL flag. So using this*/
int recv_all(int socket,int len, char *v){
    int n,got;
    wait_buffer_t wait_buffer;
    
    wait_buffer.curr=wait_buffer.buffer=v;
    do{
	got=wait_buffer.curr-wait_buffer.buffer;
	n=recv_to_wait_buffer(socket,len-got,&wait_buffer);
    }while ( n==-2 );
    n=(n<0)?-1:n;
    return n;
}

void setup_tun_bundle(int socket, unsigned long int n){
    tun_bundle_t ** bnd;
    int i;
    init_packet_t init_packet;
    
    debug(10,0,"setup_tun_bundle()");
    if(n>TUN_CONNECTIONS_LIMIT || n<2){
	debug(3,0,"setup_tun_bundle: Wrong tunnel connections number. Ignoring connection");
	shutdown(socket,SHUT_RDWR);
	close(socket);
	return;
    }

    pthread_mutex_lock(&bundles_mutex);

    for(bnd=&bundles; *bnd!=NULL; bnd=&(bnd[0]->next));
    
    init_packet.m='O';
    init_packet.n=bundles_current_new_id;
    i=send(socket, (const void *)&init_packet, sizeof(init_packet), MSG_NOSIGNAL);
    if(i==-1){
	debug(0,errno,"setup_tun_bundle: send");
	shutdown(socket,SHUT_RDWR);
	close(socket);
	pthread_mutex_unlock(&bundles_mutex);
	return;
    }
    
    bnd[0]=(tun_bundle_t *)malloc(sizeof(tun_bundle_t));
    bnd[0]->next=NULL;
    bnd[0]->bundle_id=bundles_current_new_id;
    bnd[0]->start_time=time(NULL);
    bnd[0]->arrived=1;
    bnd[0]->thread=0;
    bnd[0]->tun_connections_number=n;
    bundles_current_new_id++;
    debug(10,0,"setup_tun_bundle: bundles_current_new_id=%lu",bundles_current_new_id);
    bnd[0]->pollfd=(struct pollfd *)malloc(sizeof(struct pollfd)*(n+1));
    debug(10,0,"setup_tun_bundle: Got pollfd=%p from malloc",bnd[0]->pollfd);
    bnd[0]->pollfd[1].fd=socket;
    bnd[0]->pollfd[1].events=recv_mask;
    
    debug(10,0,"setup_tun_bundle: Setted up bnd[0]=%p id=%lu",bnd[0],bnd[0]->bundle_id);
    pthread_mutex_unlock(&bundles_mutex);
}

void destroy_bundle(tun_bundle_t ** bnd){
    int i;
    tun_bundle_t *bt;
    
    debug(10,0,"destroy_bundle %p for thread %lu",bnd[0],bnd[0]->thread);
    bt=bnd[0]->next;
    if((bnd[0]->arrived == bnd[0]->tun_connections_number) &&
	(bnd[0]->start_time == 0)){
	
	shutdown(bnd[0]->pollfd[0].fd,SHUT_RDWR);
	close(bnd[0]->pollfd[0].fd);
    }
    for(i=1;i<=bnd[0]->arrived;i++){
	shutdown(bnd[0]->pollfd[i].fd,SHUT_RDWR);
	close(bnd[0]->pollfd[i].fd);
    }
    free(bnd[0]->pollfd);
    free(*bnd);
    *bnd=bt;
}


ret_t one_to_many(int sd, int send_sd, seq_id_t *seq_id){
    char buf[MAX_TUNNEL_PACKET_DATA_SIZE];
    packet_t packet;
    ret_t ret;
    
    debug(5,0,"one_to_many");
    ret.s2=1;
    ret.s1=recv(sd,buf,MAX_TUNNEL_PACKET_DATA_SIZE,0);
    if(ret.s1>0){
	packet.id=seq_id->one_to_many;
	packet.data_len=ret.s1;
	ret.s2=send(send_sd, &packet, sizeof(packet_t), MSG_NOSIGNAL);
	if(ret.s2==sizeof(packet_t)){
	    ret.s2=send(send_sd, buf, packet.data_len, MSG_NOSIGNAL);
	    if(ret.s2==packet.data_len){
		seq_id->one_to_many++;
	    }else{
		if(ret.s2>0){
		    debug(0,0,"one_to_many: send data: sent less (%d) then asked (%d). Shutdown %d forced",ret.s2,sizeof(packet_t),send_sd);
		    ret.s2=0;
		}else
		    debug(0,errno,"one_to_many: send data");
	    }
	}else{
	    if(ret.s2>0){
		debug(0,0,"one_to_many: send packet: sent less (%d) then asked (%d). Shutdown %d forced",ret.s2,sizeof(packet_t),send_sd);
		ret.s2=0;
	    }else
		debug(0,errno,"one_to_many: send packet");
	}
    }else
	if (ret.s1!=0) debug(0,errno,"one_to_many: recv");
    debug(5,0,"one_to_many: done %d %d",ret.s1,ret.s2);
    return ret;
}

int check_many_to_one_send_queue(send_buffer_t ** send_buffer, int send_sd, seq_id_t *seq_id){
    send_buffer_t **s, *sfree;
    int ret;

    debug(5,0,"check_many_to_one_send_queue");
    ret=1;
    for(s=send_buffer; s[0]!=NULL; ){
	if(s[0]->packet.id==seq_id->many_to_one){
	    ret=send(send_sd, s[0]->data, s[0]->packet.data_len, MSG_NOSIGNAL);
	    if(ret==s[0]->packet.data_len){
		seq_id->many_to_one++;
		free(s[0]->data);
		sfree=s[0];
		s[0]=s[0]->next;
		free(sfree);
		s=send_buffer;
	    }else{
		if(ret>0){
		    debug(0,0,"check_many_to_one_send_queue: send: sent less (%d) then asked (%lu). Shutdown %d forced",ret,s[0]->packet.data_len,send_sd);
		    ret=0;
		}else
		    debug(0,errno,"many_to_one: send");
		break;
	    }
	}else
	    s=&(s[0]->next);
    }
    debug(5,0,"check_many_to_one_send_queue: end");
    return ret;
}

int add_to_send_queue(send_buffer_t **send_buffer, packet_t *packet){
    send_buffer_t **s;

    debug(5,0,"add_to_send_queue");
    for(s=send_buffer;s[0]!=NULL;s=&(s[0]->next));
    s[0]=(send_buffer_t*)malloc(sizeof(send_buffer_t));
    s[0]->next=NULL;
    s[0]->packet=*packet;
    s[0]->data=(char *)malloc(sizeof(char)*packet->data_len);
    memcpy(s[0]->data,(char*)packet+sizeof(packet_t),packet->data_len);
    debug(5,0,"add_to_send_queue: end");
    return 1;
}

ret_t many_to_one(int sd, int send_sd, send_buffer_t **send_buffer, wait_buffer_t *wait_buffer, seq_id_t *seq_id){
    packet_t * packet;
    ret_t ret;
    
    debug(5,0,"many_to_one");
    ret.s2=1;    
    ret.s1=recv_to_wait_buffer(sd, sizeof(packet_t), wait_buffer);
    if(ret.s1>0 || ret.s1==-3){
	packet=(packet_t*)wait_buffer->buffer;
	ret.s1=recv_to_wait_buffer(sd, packet->data_len+sizeof(packet_t), wait_buffer);
	if(ret.s1>0){
	    wait_buffer->curr=wait_buffer->buffer;
	    if(packet->id==seq_id->many_to_one){
		ret.s2=send(send_sd, (char*)packet + sizeof(packet_t),packet->data_len, MSG_NOSIGNAL);
		if(ret.s2==packet->data_len){
		    seq_id->many_to_one++;
		    ret.s2=check_many_to_one_send_queue(send_buffer,send_sd,seq_id);
		}else{
		    if(ret.s2>0){
			debug(0,0,"many_to_one: send packet: sent less (%d) them asked (%lu). Sshutdown %d forced",ret.s2,packet->data_len,send_sd);
			ret.s2=0;
		    }else
			debug(0,errno,"many_to_one: send");
		}
	    }else
		ret.s2=add_to_send_queue(send_buffer, packet);
	}else{
	    switch (ret.s1){
		case -3:
		    debug(0,0,"many_to_one: recv data %d: Problem in wait_queue!",ret.s1);
		    ret.s1=-1;
		    break;
		case -2:
		    ret.s1=1;
		    break;
		case 0:
		    break;
		default:
		    debug(0,errno,"many_to_one: recv data");
	    }
	}
    }else{
	if(ret.s1!=-2){
	    if(ret.s1!=0) debug(0,errno,"many_to_one: recv");
	}else
	    ret.s1=1;
    }
    debug(5,0,"many_to_one: done %d %d",ret.s1,ret.s2);
    return ret;
}


int work_with_poll(struct pollfd *pollfd, int cn){
    int flag,i,j,n;
    send_buffer_t * send_buffer, *sfree;
    ret_t ret;
    seq_id_t seq_id;
    queue_order_t qorder;
    wait_buffer_t * wait_buffers;
    

    debug(10,0,"work_with_poll (pollfd=%p, cn=%d)",pollfd,cn);
    qorder.current=1;
    qorder.total=cn;
    
    send_buffer=NULL;
    
    seq_id.many_to_one=0;
    seq_id.one_to_many=0;

    wait_buffers=(wait_buffer_t *)malloc(sizeof(wait_buffer_t)*(cn+1));
    wait_buffers[0].buffer=NULL;
    wait_buffers[0].curr=NULL;
    for(i=1;i<=cn;i++){
	wait_buffers[i].curr=wait_buffers[i].buffer=(char*)malloc(sizeof(char)*MAX_TUNNEL_PACKET_SIZE);
    }

    flag=1;
    while((flag==1) && ((n=poll(pollfd, cn+1, CLIENT_POLL_SOCKET_TIMEOUT*1000))>0)){
	flag=0;
	for(i=0;i<=cn;i++){
	    
	    if(pollfd[i].revents & POLLNVAL){
		for(j=0;j<=cn;j++) pollfd[j].events=0;
		flag=-1;
		break;
	    }
	    
	    if(pollfd[i].revents & (POLLERR | POLLHUP)){
		pollfd[i].events&=~send_mask;
		if(i==0){
		    for(j=1;j<=cn;j++) pollfd[j].events&=~recv_mask;
		}else{
		    pollfd[0].events&=~recv_mask;
		}
	    }
	    
	    if(pollfd[i].revents & recv_mask){
		if(i==0){
		    ret=one_to_many(pollfd[i].fd, pollfd[qorder.current].fd, &seq_id);
		    qorder.current++;
		    if(qorder.current>qorder.total) qorder.current=1;
		    if(ret.s1<0 || ret.s2<0){
			flag=-1;
			break;
		    }
		    if(ret.s1==0){
			flag=0;
			break;
		    }
		    if(ret.s2==0){
//			check_many_to_one_send_queue(send_buffer,pollfd[0].fd);
			flag=0;
			break;
		    }
		}else{
		    ret=many_to_one(pollfd[i].fd, pollfd[0].fd, &send_buffer, wait_buffers+i, &seq_id);
		    if(ret.s1<0 || ret.s2<0){
			flag=-1;
			break;
		    }
		    if(ret.s1==0){
			pollfd[i].events&=~recv_mask;
		    }
		    if(ret.s2==0){
			flag=0;
			break;
		    }
		}
	    }
	    
	    flag|=pollfd[i].events;
	    
	}
	if(flag==-1) break;
	if(flag!=0) flag=1;
    }
    
    for(i=1;i<=cn;i++)	free(wait_buffers[i].buffer);
    free(wait_buffers);
    
    for(;send_buffer!=NULL;){
	sfree=send_buffer;
	send_buffer=send_buffer->next;
	free(sfree);
    }
    debug(6,0,"work_with_poll: end");
    return 0;
}

void * bundle_thread(void *arg){
    tun_bundle_t *bnd_arg;
    tun_bundle_t ** bnd;
    pthread_t thread;

    bnd_arg=(tun_bundle_t *)arg;
    debug(5,0,"bundle_thread(bnd_arg=%p)",bnd_arg);

    bnd_arg->pollfd[0].events=recv_mask;
    bnd_arg->pollfd[0].fd=start_connected_socket(&args.external_addr);

    if(bnd_arg->pollfd[0].fd!=-1){
	bnd_arg->start_time=0;
	work_with_poll(bnd_arg->pollfd, bnd_arg->tun_connections_number);
    }
    
    thread=pthread_self();
    pthread_mutex_lock(&bundles_mutex);

    for(bnd=&bundles; *bnd!=NULL; ){
        if(bnd[0]->thread == thread && bnd[0]->start_time==0){
	    destroy_bundle(bnd);
	}else{
	    bnd=&(bnd[0]->next);
	}
    }

    pthread_mutex_unlock(&bundles_mutex);
    debug(5,0,"bundle_thread: thread exit");
    pthread_exit(NULL);
    return NULL;
}

void process_tun_bundle(void){
    tun_bundle_t ** bnd;
    pthread_t thread;
    
    if(bundles==NULL) return; /* For client part to do not go deep */
    debug(10,0,"process_tun_bundle()");
    
    pthread_mutex_lock(&bundles_mutex);

    for(bnd=&bundles; *bnd!=NULL; ){
	if(bnd[0]->arrived == bnd[0]->tun_connections_number){
	    if(bnd[0]->start_time != 0){
		/* Main assamblin-disassembling thread starting */
		if (pthread_create(&thread, NULL, bundle_thread, bnd[0])!=0){
		    debug(0,errno,"Thread creation failed");
		    destroy_bundle(bnd);
		    continue;
		}
		if(pthread_detach(thread)!=0){
		    debug(0,errno,"Thread detach failed");
		    destroy_bundle(bnd);
		    continue;
		}
		bnd[0]->thread=thread;
		bnd[0]->start_time=0;
	    }
	}else{
	    if((time(NULL) - bnd[0]->start_time > TUN_BUNDLE_TIMEOUT)){
		/* Hope thread will cancel by closed socket I/O operations, so do not call pthread_cancel */
		destroy_bundle(bnd);
		debug(3,0,"process_tun_bundle: bundle %lu timed out",bnd[0]->bundle_id);
		continue;
	    }
	}
	bnd=&(bnd[0]->next);
    }
    
    pthread_mutex_unlock(&bundles_mutex);
}

void attach_tun_bundle(int socket, unsigned long int n){
    tun_bundle_t ** bnd;
    
    debug(5,0,"attach_tun_bundle(socket=%d, n=%d)",socket,n);
    pthread_mutex_lock(&bundles_mutex);

    for(bnd=&bundles; *bnd!=NULL; bnd=&(bnd[0]->next))
	if(bnd[0]->bundle_id==n) break;
    
    if(*bnd==NULL){
	debug(3,0,"attach_tun_bundle: Ignore wrong packet, no id in tun_bundle");
	shutdown(socket,SHUT_RDWR);
	close(socket);
	pthread_mutex_unlock(&bundles_mutex);
	return;
    }
    
    if(bnd[0]->arrived>=bnd[0]->tun_connections_number){
	debug(3,0,"attach_tun_bundle: Exess packet, tun_bundle complited");
	shutdown(socket,SHUT_RDWR);
	close(socket);
	pthread_mutex_unlock(&bundles_mutex);
	return;
    }
    
    bnd[0]->arrived++;
    bnd[0]->start_time=time(NULL);/* Update to last packet time */
    bnd[0]->pollfd[bnd[0]->arrived].fd=socket;
    bnd[0]->pollfd[bnd[0]->arrived].events=recv_mask;
    
    pthread_mutex_unlock(&bundles_mutex);
    debug(10,0,"attach_tun_bundle: attached to bundle %p",bnd[0]);

    process_tun_bundle();
    debug(5,0,"attach_tun_bundle: end");
}

/* Server main callback function */
void * new_tun_client_connected(void * arg){
    int socket;
    thread_info_t * thi;
    char other_part_addr[16];
    init_packet_t init_packet;
    int n;
    
    thi=(thread_info_t*)arg;
    socket=thi->fd;    
    inet_ntop(AF_INET, &(thi->other_part_addr.sin_addr), other_part_addr, 15);
    debug(5,0,"new_tun_client_connected socket(%d), host(%s:%d)",
	socket,other_part_addr, ntohs(thi->other_part_addr.sin_port));
    free(thi);
    

    n=recv_all(socket,sizeof(init_packet_t),(char*)&init_packet);
    if(n<=0){
	if(n==0)
	    debug(0,0,"new_tun_client_connected: recv: Unexpected shutdown of connection");
	shutdown(socket,SHUT_RDWR);
	close(socket);
    }else{
	    switch(init_packet.m){
		case 'N':
		    setup_tun_bundle(socket,init_packet.n);
		    break;
		case 'n':
		    debug(3,0,"new_tun_client_connected: inittial packet id %lu",init_packet.n);
		    attach_tun_bundle(socket,init_packet.n);
		    break;
		default:
		    debug(3,0,"new_tun_client_connected: Wrong first char (%X) of inittial packet",init_packet.m);
		    shutdown(socket,SHUT_RDWR);
		    close(socket);
	    }
    }

    debug(10,0,"new_tun_client_connected: Thread exit");
    pthread_exit(NULL);
    return NULL;
}

/* Client main callback function */
void * new_local_client_connected(void * arg){
    int socket,i,n;
    struct pollfd * pollfd;
    init_packet_t init_packet;
    thread_info_t * thi;
    char other_part_addr[16];
    
    thi=(thread_info_t*)arg;
    socket=thi->fd;
    inet_ntop(AF_INET, &(thi->other_part_addr.sin_addr), other_part_addr, 15);
    debug(5,0,"new_local_client_connected socket(%d), host(%s:%d)",
		socket,other_part_addr, ntohs(thi->other_part_addr.sin_port));
    free(thi);
    
    pollfd=(struct pollfd *)malloc(sizeof(struct pollfd)*(args.connections_number+1));
    pollfd[0].events=recv_mask;
    pollfd[0].fd=socket;
    
    init_packet.m='N';
    init_packet.n=args.connections_number;
    for(i=1;i<=args.connections_number;i++){
	pollfd[i].events=recv_mask;
	pollfd[i].fd=start_connected_socket(&args.external_addr);	
	if(pollfd[i].fd==-1) break;
	debug(10,0,"new_local_client_connected: opening tunnel connection %d",i);
	    
	n=send(pollfd[i].fd, (const void *)&init_packet, sizeof(init_packet), MSG_NOSIGNAL);
	if(n==-1){
	    debug(0,errno,"new_local_client_connected: send");
	    shutdown(pollfd[i].fd,SHUT_RDWR);
	    close(pollfd[i].fd);
	    break;
	}

	if(i==1){
	    n=recv_all(pollfd[i].fd, sizeof(init_packet_t), (char*)&init_packet);
	    if(n<=0){
		if(n==0)
		    debug(0,0,"new_local_client_connected: recv: Unexpected shutdown of connection");
		shutdown(pollfd[i].fd,SHUT_RDWR);
		close(pollfd[i].fd);
		break;
	    }
	    if(init_packet.m!='O'){
		debug(0,0,"new_local_client_connected: Illegal answer on init packet (%X)",init_packet.m);
		shutdown(pollfd[i].fd,SHUT_RDWR);
		close(pollfd[i].fd);
		break;
	    }
	    init_packet.m='n';
	    debug(10,0,"new_local_client_connected: got init_packet id=%lu",init_packet.n);
	}
    }
    
    /* Check if all sockets connected successfuly */
    if(i==args.connections_number+1)
	work_with_poll(pollfd, args.connections_number);

    /* Close successfuly connected sockets */
    for(i--;i>0;i--){
	shutdown(pollfd[i].fd,SHUT_RDWR);
	close(pollfd[i].fd);
    }
    shutdown(socket,SHUT_RDWR);
    close(socket);

    free(pollfd);
    debug(10,0,"thread finished");
    pthread_exit(NULL);
    return NULL;
}

int start_listen_socket(struct sockaddr_in * addr, void * cb(void*)){
    int i,csa_len;
    int socket_fd;
    struct sockaddr_in csa;
    int csfd;
    pthread_t thread;
    thread_info_t * th_inf;
    struct timeval t;
    
    if ((socket_fd=open_new_socket())==-1){
	return 0;
    }
    if (bind(socket_fd,(const struct sockaddr *)addr,sizeof(struct sockaddr_in))==0){
	/* socked binded. Turn on listening */
	if (listen(socket_fd, LISTEN_BACKLOG) != 0){
	    debug(0,errno,"listen");
	    close(socket_fd);
	    return 0;
	}
    }else{
	debug(0,errno,"bind");
	close(socket_fd);
	return 0;
    }

    csa_len=sizeof(struct sockaddr_in);
    t.tv_usec=0;
    /* Main loop */
    while (1==1){
	if ((csfd=accept(socket_fd,(struct sockaddr *)&csa,(unsigned int *)&csa_len))==-1){
	    debug(0,errno,"accept");
	    close(socket_fd);
	    return 0;
	}
	t.tv_sec=SOCKET_RECIVE_TIMEOUT;
	if (setsockopt(csfd,SOL_SOCKET,SO_RCVTIMEO,(char *)&t,sizeof(t))!=0){
	    debug(0,errno,"start_connected_socket: setsockopt");
	    close(csfd);
	    continue;
	}
	t.tv_sec=SOCKET_SEND_TIMEOUT;
	if (setsockopt(csfd,SOL_SOCKET,SO_SNDTIMEO,(char *)&t,sizeof(t))!=0){
	    debug(0,errno,"start_connected_socket: setsockopt");
	    close(csfd);
	    continue;
	}
	/* Threads creation */
	th_inf=(thread_info_t*)malloc(sizeof(thread_info_t));
	th_inf->fd=csfd;
	th_inf->other_part_addr=csa;
	
	if ((i=pthread_create(&thread, NULL, cb, th_inf))!=0){
	    debug(0,errno,"Thread creation failed");
	    return 0;
	}
	if(pthread_detach(thread)!=0){
	    debug(0,errno,"Thread detach failed");
	    return 0;
	}
	
    }
    
    /* Should never go here */
    close(socket_fd);
    return 1;
}

void usage(char *a){
    fprintf(stderr,"Usage: %s -L X.X.X.X:P1 -R Y.Y.Y.Y:P2 {-c [-t n] | -s}\n\n\
    Example:\t%s -L 127.0.0.1:110 -R 192.168.1.100:12345 -c -t10\n\t\tor\n\
    \t\t%s -L 192.168.1.100:12345 -R 127.0.0.1:110 -s\n", a, a, a);
}

int parse_addr(struct sockaddr_in * a,char * s){
    char * si;
    struct addrinfo * sai,*sai0,hints_ai;
    int i;
    
    if (s==NULL){
	debug(0,0,"No adress given");
	return 0;
    }
    for(si=s;*si!='\0' && *si!=':';si++);
    if (*si!=':'){
	debug(0,0,"No port given");
	return 0;
    }
    *si='\0';
    
    hints_ai.ai_family=AF_INET;
    hints_ai.ai_protocol=0;
    hints_ai.ai_socktype=0;
    hints_ai.ai_flags=AI_V4MAPPED | AI_ADDRCONFIG;
    if((i=getaddrinfo(s,si+1,&hints_ai,&sai0))!=0){
	debug(0,-i,"getaddrinfo");
	*si=':';	
	return 0;
    }
    *si=':';
    for(sai=sai0;(sai!=NULL) && (((struct sockaddr_in *)(sai->ai_addr))->sin_port==0); sai=sai->ai_next);
    if (sai==NULL){
	freeaddrinfo(sai0);
	debug(0,0,"Cant find sutable address");
	return 0;
    }
    *a=*((struct sockaddr_in *)(sai->ai_addr));
    freeaddrinfo(sai0);
    return 1;
}

int parse_args(int argc,char ** argv){
    int opt,flag;

    flag=1;
    args.mode=NONE_MODE;
    args.external_addr.sin_addr.s_addr=INADDR_ANY;
    args.internal_addr.sin_addr.s_addr=INADDR_ANY;
    args.connections_number=0;
    while ((flag==1) && ((opt = getopt(argc, argv, "L:R:sct:")) != -1)) {
	switch (opt) {
	    case 'L':
		flag=parse_addr(&args.internal_addr,optarg);
	        break;
	    case 'R':
		flag=parse_addr(&args.external_addr,optarg);
	        break;
	    case 's':
		if (args.mode!=NONE_MODE) {
		    debug(0,0,"Only one option -s or -c allowed");
		    flag=0;
		}else{
		    args.mode=SERVER_MODE;
		}
		break;
	    case 'c':
		if (args.mode!=NONE_MODE) {
		    debug(0,0,"Only one option -s or -c allowed");
		    flag=0;
		}else{
		    args.mode=CLIENT_MODE;
		}
		break;
	    case 't':	    
		args.connections_number=atoi(optarg);
		flag=(args.connections_number>1 && 
		    args.connections_number<=TUN_CONNECTIONS_LIMIT);
		if(!flag)
		    debug(0,0,"Option -t out of range (2-%d)",TUN_CONNECTIONS_LIMIT);
	        break;
            default: 
		debug(0,0,"Unknown option!");
		flag=0;
	}
    }
    flag=(flag && args.mode!=NONE_MODE);
//    flag=(flag && args.external_addr.sin_addr.s_addr!=INADDR_ANY && args.internal_addr.sin_addr.s_addr!=INADDR_ANY);
    flag=(flag && args.external_addr.sin_addr.s_addr!=INADDR_ANY);
    flag=(flag && ((args.mode==SERVER_MODE && args.connections_number==0)|(args.mode==CLIENT_MODE && args.connections_number!=0)));
    if (! flag) usage(argv[0]);
    return flag;
}

int main(int argc,char ** argv){

    if(MAX_TUNNEL_PACKET_SIZE<=TUNNEL_PACKET_HEADER_SIZE){
	debug(0,0,"Check debug directives MAX_TUNNEL_PACKET_SIZE (%lu) should be greater than TUNNEL_PACKET_HEADER_SIZE (%lu)!",MAX_TUNNEL_PACKET_SIZE,TUNNEL_PACKET_HEADER_SIZE);
	return 1;
    }
    
    if (! parse_args(argc,argv)){
	debug(0,0,"Arguments parsing failed! Stopping execution.");
	return 1;
    }

    pthread_mutex_init(&bundles_mutex,NULL);
    pthread_mutex_init(&debug_mutex,NULL);
    
    if(args.mode==SERVER_MODE){
	start_listen_socket(&args.internal_addr,new_tun_client_connected);
    }else{
	/* args.mode==CLIENT_MODE */
	start_listen_socket(&args.internal_addr,new_local_client_connected);
    }
    
    pthread_mutex_destroy(&bundles_mutex);
    return 0;
}