#include <errno.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <malloc.h>
#include <vsprintf.h>
#include <byteswap.h>
#include <gpxe/in.h>
#include <gpxe/ip6.h>
#include <gpxe/ndp.h>
#include <gpxe/list.h>
#include <gpxe/icmp6.h>
#include <gpxe/tcpip.h>
#include <gpxe/socket.h>
#include <gpxe/pkbuff.h>
#include <gpxe/netdevice.h>
#include <gpxe/if_ether.h>

struct net_protocol ipv6_protocol;

/* Unspecified IP6 address */
static struct in6_addr ip6_none = {
        .in6_u.u6_addr32[0] = 0,
        .in6_u.u6_addr32[1] = 0,
        .in6_u.u6_addr32[2] = 0,
        .in6_u.u6_addr32[3] = 0,
};

/** An IPv6 routing table entry */
struct ipv6_miniroute {
	/* List of miniroutes */
	struct list_head list;

	/* Network device */
	struct net_device *netdev;
	/** Reference to network device */
	struct reference netdev_ref;

	/* Destination prefix */
	struct in6_addr prefix;
	/* Prefix length */
	int prefix_len;
	/* IPv6 address of interface */
	struct in6_addr address;
	/* Gateway address */
	struct in6_addr gateway;
};

/** List of IPv6 miniroutes */
static LIST_HEAD ( miniroutes );

static void ipv6_forget_netdev ( struct reference *ref );

/**
 * Add IPv6 minirouting table entry
 *
 * @v netdev		Network device
 * @v prefix		Destination prefix
 * @v address		Address of the interface
 * @v gateway		Gateway address (or ::0 for no gateway)
 * @ret miniroute	Routing table entry, or NULL
 */
static struct ipv6_miniroute * add_ipv6_miniroute ( struct net_device *netdev,
						    struct in6_addr prefix,
						    int prefix_len,
						    struct in6_addr address,
						    struct in6_addr gateway ) {
	struct ipv6_miniroute *miniroute;
	
	miniroute = malloc ( sizeof ( *miniroute ) );
	if ( miniroute ) {
		/* Record routing information */
		miniroute->netdev = netdev;
		miniroute->prefix = prefix;
		miniroute->prefix_len = prefix_len;
		miniroute->address = address;
		miniroute->gateway = gateway;
		
		/* Add miniroute to list of miniroutes */
		if ( !IP6_EQUAL ( gateway, ip6_none ) ) {
			list_add_tail ( &miniroute->list, &miniroutes );
		} else {
			list_add ( &miniroute->list, &miniroutes );
		}

		/* Record reference to net_device */
		miniroute->netdev_ref.forget = ipv6_forget_netdev;
		ref_add ( &miniroute->netdev_ref, &netdev->references );
	}

	return miniroute;
}

/**
 * Delete IPv6 minirouting table entry
 *
 * @v miniroute		Routing table entry
 */
static void del_ipv6_miniroute ( struct ipv6_miniroute *miniroute ) {
	ref_del ( &miniroute->netdev_ref );
	list_del ( &miniroute->list );
	free ( miniroute );
}

/**
 * Forget reference to net_device
 *
 * @v ref		Persistent reference
 */
static void ipv6_forget_netdev ( struct reference *ref ) {
	struct ipv6_miniroute *miniroute
		= container_of ( ref, struct ipv6_miniroute, netdev_ref );

	del_ipv6_miniroute ( miniroute );
}

/**
 * Add IPv6 interface
 *
 * @v netdev	Network device
 * @v prefix	Destination prefix
 * @v address	Address of the interface
 * @v gateway	Gateway address (or ::0 for no gateway)
 */
int add_ipv6_address ( struct net_device *netdev, struct in6_addr prefix,
		       int prefix_len, struct in6_addr address,
		       struct in6_addr gateway ) {
	struct ipv6_miniroute *miniroute;

	/* Clear any existing address for this net device */
	del_ipv6_address ( netdev );

	/* Add new miniroute */
	miniroute = add_ipv6_miniroute ( netdev, prefix, prefix_len, address,
					 gateway );
	if ( ! miniroute )
		return -ENOMEM;

	return 0;
}

/**
 * Remove IPv6 interface
 *
 * @v netdev	Network device
 */
void del_ipv6_address ( struct net_device *netdev ) {
	struct ipv6_miniroute *miniroute;

	list_for_each_entry ( miniroute, &miniroutes, list ) {
		if ( miniroute->netdev == netdev ) {
			del_ipv6_miniroute ( miniroute );
			break;
		}
	}
}

/**
 * Calculate TCPIP checksum
 *
 * @v pkb	Packet buffer
 * @v tcpip	TCP/IP protocol
 *
 * This function constructs the pseudo header and completes the checksum in the
 * upper layer header.
 */
static uint16_t ipv6_tx_csum ( struct pk_buff *pkb, uint16_t csum ) {
	struct ip6_header *ip6hdr = pkb->data;
	struct ipv6_pseudo_header pshdr;

	/* Calculate pseudo header */
	memset ( &pshdr, 0, sizeof ( pshdr ) );
	pshdr.src = ip6hdr->src;
	pshdr.dest = ip6hdr->dest;
	pshdr.len = htons ( pkb_len ( pkb ) - sizeof ( *ip6hdr ) );
	pshdr.nxt_hdr = ip6hdr->nxt_hdr;

	/* Update checksum value */
	return tcpip_continue_chksum ( csum, &pshdr, sizeof ( pshdr ) );
}

/**
 * Dump IP6 header for debugging
 *
 * ip6hdr	IPv6 header
 */
void ipv6_dump ( struct ip6_header *ip6hdr ) {
	DBG ( "IP6 %p src %s dest %s nxt_hdr %d len %d\n", ip6hdr,
	      inet6_ntoa ( ip6hdr->src ), inet6_ntoa ( ip6hdr->dest ),
	      ip6hdr->nxt_hdr, ntohs ( ip6hdr->payload_len ) );
}

/**
 * Transmit IP6 packet
 *
 * pkb		Packet buffer
 * tcpip	TCP/IP protocol
 * st_dest	Destination socket address
 *
 * This function prepends the IPv6 headers to the payload an transmits it.
 */
static int ipv6_tx ( struct pk_buff *pkb,
		     struct tcpip_protocol *tcpip,
		     struct sockaddr_tcpip *st_dest,
		     struct net_device *netdev,
		     uint16_t *trans_csum ) {
	struct sockaddr_in6 *dest = ( struct sockaddr_in6* ) st_dest;
	struct in6_addr next_hop;
	struct ipv6_miniroute *miniroute;
	uint8_t ll_dest_buf[MAX_LL_ADDR_LEN];
	const uint8_t *ll_dest = ll_dest_buf;
	int rc;

	/* Construct the IPv6 packet */
	struct ip6_header *ip6hdr = pkb_push ( pkb, sizeof ( *ip6hdr ) );
	memset ( ip6hdr, 0, sizeof ( *ip6hdr) );
	ip6hdr->ver_traffic_class_flow_label = htonl ( 0x60000000 );//IP6_VERSION;
	ip6hdr->payload_len = htons ( pkb_len ( pkb ) - sizeof ( *ip6hdr ) );
	ip6hdr->nxt_hdr = tcpip->tcpip_proto;
	ip6hdr->hop_limit = IP6_HOP_LIMIT; // 255

	/* Determine the next hop address and interface
	 *
	 * TODO: Implement the routing table.
	 */
	next_hop = dest->sin6_addr;
	list_for_each_entry ( miniroute, &miniroutes, list ) {
		if ( ( strncmp ( &ip6hdr->dest, &miniroute->prefix,
					miniroute->prefix_len ) == 0 ) ||
		     ( IP6_EQUAL ( miniroute->gateway, ip6_none ) ) ) {
			netdev = miniroute->netdev;
			ip6hdr->src = miniroute->address;
			if ( ! ( IS_UNSPECIFIED ( miniroute->gateway ) ) ) {
				next_hop = miniroute->gateway;
			}
			break;
		}
	}
	/* No network interface identified */
	if ( !netdev ) {
		DBG ( "No route to host %s\n", inet6_ntoa ( ip6hdr->dest ) );
		rc = -EHOSTUNREACH;
		goto err;
	}

	/* Complete the transport layer checksum */
	if ( trans_csum )
		*trans_csum = ipv6_tx_csum ( pkb, *trans_csum );

	/* Print IPv6 header */
	ipv6_dump ( ip6hdr );
	
	/* Resolve link layer address */
	if ( next_hop.in6_u.u6_addr8[0] == 0xff ) {
		ll_dest_buf[0] = 0x33;
		ll_dest_buf[1] = 0x33;
		ll_dest_buf[2] = next_hop.in6_u.u6_addr8[12];
		ll_dest_buf[3] = next_hop.in6_u.u6_addr8[13];
		ll_dest_buf[4] = next_hop.in6_u.u6_addr8[14];
		ll_dest_buf[5] = next_hop.in6_u.u6_addr8[15];
	} else {
		/* Unicast address needs to be resolved by NDP */
		if ( ( rc = ndp_resolve ( netdev, &next_hop, &ip6hdr->src,
					  ll_dest_buf ) ) != 0 ) {
			DBG ( "No entry for %s\n", inet6_ntoa ( next_hop ) );
			goto err;
		}
	}

	/* Transmit packet */
	return net_tx ( pkb, netdev, &ipv6_protocol, ll_dest );

  err:
	free_pkb ( pkb );
	return rc;
}

/**
 * Process next IP6 header
 *
 * @v pkb	Packet buffer
 * @v nxt_hdr	Next header number
 * @v src	Source socket address
 * @v dest	Destination socket address
 *
 * Refer http://www.iana.org/assignments/ipv6-parameters for the numbers
 */
static int ipv6_process_nxt_hdr ( struct pk_buff *pkb, uint8_t nxt_hdr,
		struct sockaddr_tcpip *src, struct sockaddr_tcpip *dest ) {
	switch ( nxt_hdr ) {
	case IP6_HOPBYHOP: 
	case IP6_ROUTING: 
	case IP6_FRAGMENT: 
	case IP6_AUTHENTICATION: 
	case IP6_DEST_OPTS: 
	case IP6_ESP: 
		DBG ( "Function not implemented for header %d\n", nxt_hdr );
		return -ENOSYS;
	case IP6_ICMP6: 
		break;
	case IP6_NO_HEADER: 
		DBG ( "No next header\n" );
		return 0;
	}
	/* Next header is not a IPv6 extension header */
	return tcpip_rx ( pkb, nxt_hdr, src, dest, 0 /* fixme */ );
}

/**
 * Process incoming IP6 packets
 *
 * @v pkb		Packet buffer
 * @v netdev		Network device
 * @v ll_source		Link-layer source address
 *
 * This function processes a IPv6 packet
 */
static int ipv6_rx ( struct pk_buff *pkb,
		     struct net_device *netdev,
		     const void *ll_source ) {

	struct ip6_header *ip6hdr = pkb->data;
	union {
		struct sockaddr_in6 sin6;
		struct sockaddr_tcpip st;
	} src, dest;

	/* Sanity check */
	if ( pkb_len ( pkb ) < sizeof ( *ip6hdr ) ) {
		DBG ( "Packet too short (%d bytes)\n", pkb_len ( pkb ) );
		goto drop;
	}

	/* TODO: Verify checksum */

	/* Print IP6 header for debugging */
	ipv6_dump ( ip6hdr );

	/* Check header version */
	if ( ip6hdr->ver_traffic_class_flow_label & 0xf0000000 != 0x60000000 ) {
		DBG ( "Invalid protocol version\n" );
		goto drop;
	}

	/* Check the payload length */
	if ( ntohs ( ip6hdr->payload_len ) > pkb_len ( pkb ) ) {
		DBG ( "Inconsistent packet length (%d bytes)\n",
			ip6hdr->payload_len );
		goto drop;
	}

	/* Ignore the traffic class and flow control values */

	/* Construct socket address */
	memset ( &src, 0, sizeof ( src ) );
	src.sin6.sin_family = AF_INET6;
	src.sin6.sin6_addr = ip6hdr->src;
	memset ( &dest, 0, sizeof ( dest ) );
	dest.sin6.sin_family = AF_INET6;
	dest.sin6.sin6_addr = ip6hdr->dest;

	/* Strip header */
	pkb_unput ( pkb, pkb_len ( pkb ) - ntohs ( ip6hdr->payload_len ) -
							sizeof ( *ip6hdr ) );
	pkb_pull ( pkb, sizeof ( *ip6hdr ) );

	/* Send it to the transport layer */
	return ipv6_process_nxt_hdr ( pkb, ip6hdr->nxt_hdr, &src.st, &dest.st );

  drop:
	DBG ( "Packet dropped\n" );
	free_pkb ( pkb );
	return -1;
}

/**
 * Print a IP6 address as xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx:xxxx
 */
char * inet6_ntoa ( struct in6_addr in6 ) {
	static char buf[40];
	uint16_t *bytes = ( uint16_t* ) &in6;
	sprintf ( buf, "%x:%x:%x:%x:%x:%x:%x:%x", bytes[0], bytes[1], bytes[2],
			bytes[3], bytes[4], bytes[5], bytes[6], bytes[7] );
	return buf;
}

static const char * ipv6_ntoa ( const void *net_addr ) {
	return inet6_ntoa ( * ( ( struct in6_addr * ) net_addr ) );
}

/** IPv6 protocol */
struct net_protocol ipv6_protocol __net_protocol = {
	.name = "IPv6",
	.net_proto = htons ( ETH_P_IPV6 ),
	.net_addr_len = sizeof ( struct in6_addr ),
	.rx = ipv6_rx,
	.ntoa = ipv6_ntoa,
};

/** IPv6 TCPIP net protocol */
struct tcpip_net_protocol ipv6_tcpip_protocol __tcpip_net_protocol = {
	.name = "IPv6",
	.sa_family = AF_INET6,
	.tx = ipv6_tx,
};