#include <linux/module.h>
#include <linux/init.h>
#include <linux/in.h>
#include <net/sock.h>

MODULE_LICENSE("GPL");

static const unsigned short server_port = 5555;
static struct socket *serversocket=NULL;
static DECLARE_COMPLETION( threadcomplete );
static int echo_server_pid;

static int create_socket( void )
{
	struct sockaddr_in server;
	int servererror;

	if( sock_create( PF_INET,SOCK_STREAM,IPPROTO_TCP,&serversocket)<0 ) {
		printk( KERN_ERR "server: Error creating serversocket.\n" );
		return -EIO;
	}
	server.sin_family = AF_INET;
	server.sin_addr.s_addr = INADDR_ANY;
	server.sin_port = htons( (unsigned short)server_port );
	servererror = serversocket->ops->bind( serversocket,
			(struct sockaddr *) &server, sizeof( server ) );
	if( servererror )
		goto release;
	servererror=serversocket->ops->listen(serversocket,3);
	if( servererror )
		goto release;
	return 0;

release:
	sock_release( serversocket );
	printk( KERN_ERR "server: Error serversocket\n" );
	return -EIO;
}

static struct socket *socket_accept( struct socket *server )
{
	struct socket *clientsocket=NULL;
	struct sockaddr address;
	int error, len;

	if( server==NULL ) return NULL;
	clientsocket = sock_alloc();
	if( clientsocket==NULL ) return NULL;

	clientsocket->type = server->type;
	clientsocket->ops = server->ops;
	error=server->ops->accept(server,clientsocket,0);
	if( error<0 ) {
		sock_release(clientsocket);
		return NULL;
	}
	error=server->ops->getname(clientsocket,
		(struct sockaddr *)&address, &len,2); 
	if( error<0 ) {
		sock_release(clientsocket);
		return NULL;
	}
	printk(KERN_INFO "new connection (%d) from %u.%u.%u.%u\n", error,
		(unsigned char)address.sa_data[2],
		(unsigned char)address.sa_data[3],
		(unsigned char)address.sa_data[4],
		(unsigned char)address.sa_data[5] );
	return clientsocket;
}

static int server_send( struct socket *sock, unsigned char *buf, int len )
{
	struct msghdr msg;
	struct iovec iov;
	mm_segment_t oldfs;

	if( sock->sk==NULL ) 
		return 0;
	iov.iov_base = buf;
	iov.iov_len  = len;
	msg.msg_control = NULL;
	msg.msg_controllen = 0;
	msg.msg_iov	 = &iov;
	msg.msg_iovlen = 1;
	msg.msg_flags = 0;

	oldfs = get_fs(); 
	set_fs( KERNEL_DS );
	len = sock_sendmsg( sock, &msg, len );
	set_fs( oldfs );
	
	return len;
}

static int server_receive( struct socket *sptr, unsigned char *buf, int len )
{
	struct msghdr msg;
	struct iovec iov;
	mm_segment_t oldfs;

	if( sptr->sk==NULL ) 
	  return 0;
	iov.iov_base = buf;
	iov.iov_len  = len;
	msg.msg_control = NULL;
	msg.msg_controllen = 0;
	msg.msg_iov	 = &iov;
	msg.msg_iovlen = 1;

	oldfs = get_fs(); 
	set_fs( KERNEL_DS );
	len = sock_recvmsg( sptr, &msg, len, 0 );
	set_fs( oldfs );
	
	return len;
}

static int echo_server( void *data )
{
	struct socket *clientsocket;
	unsigned char buffer[100];
	static int len;

	daemonize( "echo" );
	allow_signal( SIGTERM );
	while( !signal_pending(current) ) {
		clientsocket = socket_accept( serversocket );
		while( clientsocket ) {
			len = server_receive( clientsocket, buffer, sizeof(buffer) );
			if( len>0 ) {
				server_send( clientsocket, buffer, len ); // echo
			} else {
				sock_release( clientsocket );
				clientsocket=NULL;
			}
		}
	}
	complete( &threadcomplete );
	return 0;
}

static int __init server_init( void )
{
	if( create_socket()<0 )
		return -EIO;

	echo_server_pid=kernel_thread(echo_server,NULL, CLONE_KERNEL );
	if( echo_server_pid < 0 ) {
		printk(KERN_ERR "server: Error creating echo_server\n" );
		sock_release( serversocket );
		return -EIO;
	}
	return 0;
}

static void __exit server_exit( void )
{
	if( echo_server_pid ) {
		kill_proc( echo_server_pid, SIGTERM, 0 );
		wait_for_completion( &threadcomplete );
	}
	if( serversocket )
		sock_release( serversocket );
}

module_init( server_init );
module_exit( server_exit );