#include <sys/types.h>
#include <sys/uio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdio.h>

#include "anep.h"


#define BUFF_SZ 100
#define ANC_SZ  1024

#define USE_RECVFROM 1
#define USE_RECVMSG  2


server(int domain, int recv_type, int type) {
    int				sockfd;
    char			buff[BUFF_SZ + 1];
    struct sockaddr_anep	servaddr;
    struct sockaddr_anep	clntaddr;
    socklen_t			clntaddr_len = sizeof(struct sockaddr_anep);
    int				r;
    char			str[INET6_ADDRSTRLEN];
    struct anep_optype		optype[] = {
	{ ANEP_OPTYPE_KNOWN|ANEP_OPTYPE_MEMORIZE, 15, 16 }
    };
    struct msghdr		msg;
    struct iovec		iov[1];
    struct anep_optionhdr	*aoh;
    struct cmsghdr		*cmsg;
    char			*data;
    char			anc[ANC_SZ];

    cmsg = (struct cmsghdr *)anc;

    iov[0].iov_base = buff;
    iov[0].iov_len  = BUFF_SZ + 1;
    

    /* Server address */
    bzero(&servaddr, sizeof(struct sockaddr_anep));
    switch(domain) {
    case PF_INET:
	servaddr.sanep_family		= AF_INET;
	servaddr.sanep_ipv4_addr.s_addr	= htonl(INADDR_ANY);
	servaddr.sanep_type		= htons(type);
	break;
    case PF_INET6:
	servaddr.sanep_family		= AF_INET6;
	servaddr.sanep_ipv6_addr	= in6addr_any;
	servaddr.sanep_type		= htons(type);
	break;
    default:
	abort();
    }

    /* Open */
    printf("Open socket...");    fflush(stdout);
    sockfd = socket(domain, SOCK_DGRAM, SOL_ANEP);
    if (sockfd < 0) 
	goto out;
    printf("Ok\n");

    /* Setsockopt */
    printf("Setsockopt....");    fflush(stdout);
    if (setsockopt(sockfd, SOL_ANEP, ANEP_OPTYPE, optype, sizeof(optype)) < 0)
	    goto out;
    printf("Ok\n");

    /* Bind */
    printf("Bind..........");    fflush(stdout);
    if (bind(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0)
	goto out;
    printf("Ok\n");


    getsockname(sockfd, (struct sockaddr *)&clntaddr, &clntaddr_len);
    switch(domain) {
    case PF_INET:
	printf("Waiting on: %s\n", inet_ntop(clntaddr.sanep_family,
					     &(clntaddr.sanep_ipv4_addr),
					     str, 
					     INET6_ADDRSTRLEN));
	break;
    case PF_INET6:
	printf("Waiting on: %s\n", inet_ntop(clntaddr.sanep_family,
					     &(clntaddr.sanep_ipv6_addr),
					     str, 
					     INET6_ADDRSTRLEN));
	break;
    default:
	abort();
    }

    /* server loop */
    for (;;) {
	/* receive */
	switch (recv_type) {
	case USE_RECVFROM: 
	    printf("Recvfrom...");    fflush(stdout);
	    r = recvfrom(sockfd, buff, BUFF_SZ + 1, 0, 
			 (struct sockaddr *)&clntaddr, &clntaddr_len);
	    break;
	case USE_RECVMSG:
	    msg.msg_name       = (struct sockaddr *)&clntaddr;
	    msg.msg_namelen    = clntaddr_len;
	    msg.msg_iov        = iov;
	    msg.msg_iovlen     = 1;
	    msg.msg_control    = anc;
	    msg.msg_controllen = 1024;
	    msg.msg_flags      = 0;
	    
	    printf("Recvmsg...");    fflush(stdout);
	    r = recvmsg(sockfd, &msg, 0);
	    break;
	default:
	    abort();
	}

	/* error */
	if (r < 0) {
	    printf("Error: %m\n");
	    continue;
	}

	/* ok */
	printf("Ok: %d\n", r);
	    
	/* who */
	switch(domain) {
	case PF_INET:
	    printf("Contacted by: %s\n", inet_ntop(clntaddr.sanep_family,
						   &(clntaddr.sanep_ipv4_addr),
						   str, 
						   INET6_ADDRSTRLEN));
	    break;
	case PF_INET6:
	    printf("Contacted by: %s\n", inet_ntop(clntaddr.sanep_family,
						   &(clntaddr.sanep_ipv6_addr),
						   str, 
						   INET6_ADDRSTRLEN));
	    break;
	default:
	    abort();
	}
	
	/* what */
	if (r == 0) {
	    printf("Received empty ANEP datagram\n");
	} else if (r > BUFF_SZ) {
	    printf("Received too big ANEP datagram (>%d)\n", BUFF_SZ);
	} else {
	    buff[r] = 0;
	    printf("Received: %s\n", buff);
	}

	/* option? */
	if (recv_type != USE_RECVMSG)
	    continue;

	if (msg.msg_flags & MSG_TRUNC)
		printf("Control Message truncated\n");

	for (cmsg = CMSG_FIRSTHDR(&msg) ;
	     cmsg ;
	     cmsg = CMSG_NXTHDR(&msg, cmsg)) {

		if (cmsg->cmsg_level != SOL_ANEP)
			continue;

		switch (cmsg->cmsg_type) {
		case ANEP_OPTION: {
			struct	anep_optionhdr	*opthdr = 
				(struct anep_optionhdr *) CMSG_DATA(cmsg);
			__u32	 length	= ntohs(opthdr->length) << 2;
			__u16	 type	= ntohs(opthdr->type);
			
			printf("Option %d / Length %d\n", 
			       type & ANEP_OPT_TYPE_MASK, length);
			printf("  Data: %s\n", (char *)(opthdr+1));
		}
		}
	}
    }

 out:
    printf("Error: %m\n");
    exit(-1);
}



int main(int argc, char *argv[]) {
    int		type;
    char       *progname;

    progname = (char *)strrchr(argv[0], '/');
    if (!progname++)
	progname = argv[0];

    /* Arguments */
    if (argc != 2) {
	printf("usage: prog typeid\n");
	exit(0);
    }

    type     = atoi(argv[1]);

    /* Sanity check */
    if (type < 0 || type > 0xffff) {
	fprintf(stderr, "incorrect ANEP type id\n");
	exit(1);
    }

    if (!strcmp(progname, "server_v4")) {
	printf(">> Using IPv4 code\n");
	server(PF_INET, USE_RECVMSG, type);
    } else {
	printf(">> Using IPv6 code\n");
	server(PF_INET6, USE_RECVMSG, type);
    }

    exit(0);
}
