/*
 *	Simple IPv6 network layer implementation.
 *	Based and/or adapted from the IPv4 network layer in net.c
 *
 *	Copyright 2006 - 2012   Hermin Anggawijaya - Allied Telesis Labs NZ
 *	(See License)
 *
 *
 * History
 *	8/Nov/2006	  created
 *
 */

/*
 * General Desription:
 *
 * The user interface supports commands for TFTP6.
 * Also, we support Neighbour discovery internally. Depending on available data,
 * these interact as follows:
 *
 * Neighbour Discovery:
 *
 *	Prerequisites:	- own ethernet address
 *			- own IPv6 address
 *			- TFTP server IPv6 address
 *	We want:	- TFTP server ethernet address
 *	Next step:	TFTP
 *
 * TFTP over IPv6:
 *
 *	Prerequisites:	- own ethernet address
 *			- own IPv6 address
 *			- TFTP server IPv6 address
 *			- TFTP server ethernet address
 *			- name of bootfile (if unknown, we use a default name
 *			  derived from our own IPv6 address)
 *	We want:	- load the boot file
 *	Next step:	none
 *
 */
#include <common.h>
#include <watchdog.h>
#include <command.h>
#include <linux/ctype.h>
#include <net.h>
#include <net6.h>
#include "bootp.h"
#include "tftp.h"
#include "rarp.h"
#include "nfs.h"
#ifdef CONFIG_STATUS_LED
#include <status_led.h>
#include <miiphy.h>
#endif
#if defined (CONFIG_CMD_SNTP)
#include "sntp.h"
#endif

//#define ET_DEBUG

#ifdef CONFIG_IPV6

#ifdef ET_DEBUG
void debug_printf (const char *fmt, ...);
#endif

#ifdef CONFIG_NETCONSOLE
int nc_input_packet(uchar *pkt, unsigned dest, unsigned src, unsigned len);
#endif

int inet_pton4 (const char *strptr, void *addrptr);

extern uchar	PktBuf[(PKTBUFSRX+1) * PKTSIZE_ALIGN + PKTALIGN];
extern uchar 	NetArpWaitPacketBuf[PKTSIZE_ALIGN + PKTALIGN];
extern rxhand_f *packetHandler;

IP6addr_t	const NetNullAddrIP6	= {{0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00}};		/* NULL IPv6 address	*/
IP6addr_t	NetOurGatewayIP6			= {{0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00}};		/* Our gateways IPv6 address	*/
IP6addr_t	NetOurIP6							= {{0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00}};		/* Our IPv6 addr (0 = unknown)		*/
IP6addr_t	NetOurLinkLocalIP6		= {{0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00}};		/* Our link local IPv6 addr (0 = unknown)		*/
IP6addr_t	NetServerIP6					= {{0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00}};		/* set server IPv6 addr (0 = unknown)		*/

#if defined(CONFIG_CMD_PING)
IP6addr_t	NetPingIP6;		/* the ipv6 address to ping 		*/
#endif

IP6addr_t	NetNDSolPacketIP6;	/* IPv6 destination address of packet waiting for ND */
u_int32_t	NetPrefixLength = 0;/* The prefix length of our network */
IP6addr_t	NetNDRepPacketIP6;	/* IPv6 address we are expecting ND advert from */
uchar			*NetNDPacketMAC;		/* MAC destination address of packet waiting for ND */
uchar			*NetNDTxPacket;			/* pointer to packet waiting to be transmitted after ND is resolved */
int				NetNDTxPacketSize;	/* size of packet waiting to be transmitted */
ulong			NetNDTimerStart;		/* the timer for ND resolution */
int				NetNDTry;						/* the number of requests we have sent so far */


int
ip6_is_unspecified_addr (IP6addr_t *addr)
{

	return ((addr->u6_addr32[0] | addr->u6_addr32[1] | 
		addr->u6_addr32[2] | addr->u6_addr32[3] ) == 0); 

}

void
ip6_make_eui (uchar *eui, uchar *eth_add)
{
	memcpy( eui , eth_add, 3 );
	memcpy( eui + 5, eth_add+3, 3 );
	eui[3] = 0xFF;
	eui[4] = 0xFE;
	eui[0] ^= 2;		// "u" bit set to indicate global scope
}

void
ip6_make_lladdr (uchar *lladr, uchar *eth_add)
{
uchar eui[8];
__u16	hdr = 0xfe80;

	ip6_make_eui( eui, eth_add );
	memset( lladr, 0, IN6ADDRSZ );
	memcpy( lladr, &hdr, 2 );
	memcpy( lladr + 8, eui, 8 );
}

void
ip6_make_SNMA (uchar *mcast_addr, uchar *ip6_addr)
{

	memset( mcast_addr, 0, IN6ADDRSZ );
	mcast_addr[0] = 0xff;
	mcast_addr[1] = IPV6_ADDRSCOPE_LINK;
	mcast_addr[11] = 0x01;
	mcast_addr[12] = 0xff;
	mcast_addr[13] = ip6_addr[13];
	mcast_addr[14] = ip6_addr[14];
	mcast_addr[15] = ip6_addr[15];
}

void
ip6_make_mult_ethdstaddr (uchar *eth_dstaddr, uchar *mcast_addr)
{
__u16	hdr = 0x3333;

	memcpy( eth_dstaddr, &hdr, 2 );
	memcpy( eth_dstaddr+2, mcast_addr+12, 4 );
}

int
ip6_addr_in_subnet (IP6addr_t *our_addr, IP6addr_t *neigh_addr, __u32 prefix_length)
{
	__u32	* addr_dwords;
	__u32	* prefix_dwords;

	addr_dwords = our_addr->u6_addr32;
	prefix_dwords = neigh_addr->u6_addr32;

	while ( prefix_length > 32 )
	{
		if ( *addr_dwords++ != *prefix_dwords++ )
		{
			return 0;
		}
		prefix_length -= 32;
	}

/*	Check any remaining bits.
 */
	if ( (prefix_length > 0) && ((*addr_dwords >> (32 - prefix_length)) !=
			(*prefix_dwords >> (32 - prefix_length))) )
	{
		return 0;
	}

	return 1;
}

static inline unsigned int 
csum_fold (unsigned int sum)
{
	sum = (sum & 0xffff) + (sum >> 16);
	sum = (sum & 0xffff) + (sum >> 16);

	return ~sum;
}

static __u32 
csum_do_csum (const __u8 *buff, int len)
{
int odd, count;
unsigned long result = 0;

	if (len <= 0)
		goto out;
	odd = 1 & (unsigned long) buff;
	if (odd) {
		result = *buff;
		len--;
		buff++;
	}
	count = len >> 1;               /* nr of 16-bit words.. */
	if (count) {
		if (2 & (unsigned long) buff) {
			result += *(unsigned short *) buff;
			count--;
			len -= 2;
			buff += 2;
		}
		count >>= 1;            /* nr of 32-bit words.. */
		if (count) {
			unsigned long carry = 0;
			do {
				unsigned long w = *(unsigned long *) buff;
				count--;
				buff += 4;
				result += carry;
				result += w;
				carry = (w > result);
			} while (count);
			result += carry;
			result = (result & 0xffff) + (result >> 16);
		}
		if (len & 2) {
			result += *(unsigned short *) buff;
			buff += 2;
		}
	}
	if (len & 1)
		result += (*buff << 8);
	result = ~csum_fold(result);
	if (odd)
		result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
out:
	return result;
}

unsigned int
csum_partial (const unsigned char * buff, int len, unsigned int sum)
{
	unsigned int result = csum_do_csum(buff, len);

	/* add in old sum, and carry.. */
	result += sum;
	/* 16+c bits -> 16 bits */
	result = (result & 0xffff) + (result >> 16);
	return result;
}

static __inline__ unsigned short int 
csum_ipv6_magic (IP6addr_t *saddr, IP6addr_t *daddr,
	__u16 len, unsigned short proto, unsigned int csum) 
{
	int i;
	int carry;
	__u32 ulen;
	__u32 uproto;
	unsigned int finalsum;

	for (i=0; i<4; i++)
	{
		csum += saddr->u6_addr32[i];
		carry = (csum < saddr->u6_addr32[i]);
		csum += carry;

		csum += daddr->u6_addr32[i];
		carry = (csum < daddr->u6_addr32[i]);
		csum += carry;
	}

	ulen = htonl((__u32) len);
	csum += ulen;
	carry = (csum < ulen);
	csum += carry;

	uproto = htonl(proto);
	csum += uproto;
	carry = (csum < uproto);
	csum += carry;

	finalsum = csum_fold(csum);
	if ( (finalsum & 0xffff) == 0x0000 )
		return( 0xffff );
	else if ( (finalsum & 0xffff) == 0xffff )
		return( 0x0000 );
	else
		return finalsum;

}

static u8 *
ip6_ndisc_fill_addr_option (u8 *opt, int type, void *data, int data_len)
{
	int space = IP6_NDISC_OPT_SPACE(data_len);
	int pad   = 0;

	opt[0] = type;
	opt[1] = space>>3;

	memset(opt + 2, 0, pad);
	opt   += pad;
	space -= pad;

	memcpy(opt+2, data, data_len);
	data_len += 2;
	opt += data_len;
	if ((space -= data_len) > 0)
		memset(opt, 0, space);
	return opt + space;
}

static int
ip6_ndisc_extract_addr_option (u8 *opt, int type, void *eth_addr)
{

	if ( type != (int)opt[0] )
	{
		return 0;
	}

	/* assume ethernet */
	memcpy((void*)eth_addr, (void*)opt+2, 6);
	return 1;
}

int
ip6_add_hdr (volatile uchar *xip, IP6addr_t *src, IP6addr_t *dest, int nextheader,
	int hoplimit, int payload_len)
{
	volatile IP6_t *ip6 = (IP6_t *)xip;

	ip6->version = 6;
	ip6->priority = 0;
	ip6->flow_lbl[0] = 0;
	ip6->flow_lbl[1] = 0;
	ip6->flow_lbl[2] = 0;
	ip6->payload_len = htons(payload_len);
	ip6->nexthdr = nextheader;
	ip6->hop_limit = hoplimit;
	NetCopyIP6((void*)&ip6->saddr, src);
	NetCopyIP6((void*)&ip6->daddr, dest);
	
	return(IP6_HDR_SIZE);	
}


void
ip6_send_ns (IP6addr_t *neigh_addr)
{
IP6addr_t dst_adr;
uchar 		eth_dst_addr[6];
NDISC_ND_t *msg;
__u16			len;
uchar			*pkt;

	/* calculate src, dest IPv6 addr and dest Eth addr */
	ip6_make_SNMA ((uchar *)&dst_adr, (uchar *)neigh_addr );
	ip6_make_mult_ethdstaddr( eth_dst_addr, (uchar *)&dst_adr );
	len = sizeof(ICMP6_t) + IN6ADDRSZ + IP6_NDISC_OPT_SPACE(INETHADDRSZ);

	pkt = (uchar *)NetTxPacket;
	pkt += NetSetEther (pkt, eth_dst_addr, PROT_IP6);
	pkt += ip6_add_hdr(pkt, &NetOurLinkLocalIP6, &dst_adr, IPPROTO_ICMPV6, IPV6_NDISC_HOPLIMIT, len);
	
	/* ICMPv6 - NS */
	msg = (NDISC_ND_t *)pkt;
	msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_SOLICITATION;
	msg->icmph.icmp6_code = 0;
	msg->icmph.icmp6_cksum = 0;
	msg->icmph.icmp6_unused = 0;

	/* Set the target address and llsaddr option */
	NetWriteIP6(&msg->target, neigh_addr);
	ip6_ndisc_fill_addr_option(msg->opt, ND_OPT_SOURCE_LL_ADDR, NetOurEther, INETHADDRSZ);

	/* checksum */
	msg->icmph.icmp6_cksum = csum_ipv6_magic(&NetOurLinkLocalIP6,	&dst_adr, len,
		IPPROTO_ICMPV6, csum_partial((__u8 *) msg, len, 0));

	pkt += len;

#ifdef ET_DEBUG
		debug_printf("Sending NS for ");
		printf_IP6addr( neigh_addr );
		debug_printf(" to ");
		printf_IP6addr( &dst_adr );
		debug_printf("\n");
#endif

	/* send it! */
	(void) eth_send (NetTxPacket, (pkt - NetTxPacket));
}

void
ip6_send_na (uchar *eth_dst_addr, IP6addr_t *neigh_addr, IP6addr_t *target)
{
NDISC_ND_t *msg;
__u16			len;
uchar			*pkt;

	len = sizeof(ICMP6_t) + IN6ADDRSZ + IP6_NDISC_OPT_SPACE(INETHADDRSZ);

	pkt = (uchar *)NetTxPacket;
	pkt += NetSetEther (pkt, eth_dst_addr, PROT_IP6);
	pkt += ip6_add_hdr(pkt, &NetOurLinkLocalIP6, neigh_addr, IPPROTO_ICMPV6, IPV6_NDISC_HOPLIMIT, len);
	
	/* ICMPv6 - NS */
	msg = (NDISC_ND_t *)pkt;
	msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT;
	msg->icmph.icmp6_code = 0;
	msg->icmph.icmp6_cksum = 0;
	msg->icmph.icmp6_unused = 0;

	/* Set the target address and lltargetaddr option */
	NetWriteIP6(&msg->target, target);
	ip6_ndisc_fill_addr_option(msg->opt, ND_OPT_TARGET_LL_ADDR, NetOurEther, INETHADDRSZ);

	/* checksum */
	msg->icmph.icmp6_cksum = csum_ipv6_magic(&NetOurLinkLocalIP6,	neigh_addr, len,
		IPPROTO_ICMPV6, csum_partial((__u8 *) msg, len, 0));

	pkt += len;

#ifdef ET_DEBUG
		debug_printf("Sending NA for ");
		printf_IP6addr( &NetOurIP6 );
		debug_printf(" to ");
		printf_IP6addr( neigh_addr );
		debug_printf("\n");
#endif

	/* send it! */
	(void) eth_send (NetTxPacket, (pkt - NetTxPacket)/* + IP6_HDR_SIZE */);
}

int
ip6_make_ping (uchar *eth_dst_addr, IP6addr_t *neigh_addr, uchar *pkt)
{
PING6_t	  *msg;
__u16			len;
uchar 		*pkt_old = pkt;

	len = sizeof(PING6_t);

	pkt += NetSetEther (pkt, eth_dst_addr, PROT_IP6);
	pkt += ip6_add_hdr(pkt, &NetOurIP6, neigh_addr, IPPROTO_ICMPV6, IPV6_NDISC_HOPLIMIT, len);
	
	/* ICMPv6 - Echo */
	msg = (PING6_t *)pkt;
	msg->icmph.icmp6_type = IPV6_ICMP_ECHO_REQUEST;
	msg->icmph.icmp6_code = 0;
	msg->icmph.icmp6_cksum = 0;
	msg->icmph.icmp6_unused = 0;
	msg->id = htons(0xdead);
	msg->sequence = htons(0xbeef);

	/* checksum */
	msg->icmph.icmp6_cksum = csum_ipv6_magic(&NetOurIP6, neigh_addr, len,
		IPPROTO_ICMPV6, csum_partial((__u8 *) msg, len, 0));

	pkt += len;

	return( pkt - pkt_old );
}

void
ip6_NDISC_Request (void)
{

	if ( !ip6_addr_in_subnet(&NetOurIP6, &NetNDSolPacketIP6, NetPrefixLength) )
	{
		if ( ip6_is_unspecified_addr(&NetOurGatewayIP6) )
		{
			puts ("## Warning: gatewayip6 is needed but not set\n");
			NetCopyIP6((void *)&NetNDRepPacketIP6, (void *)&NetNDSolPacketIP6);
		}
		else
		{
			NetCopyIP6((void *)&NetNDRepPacketIP6, (void *)&NetOurGatewayIP6);	
		}
	}
	else
	{
		NetCopyIP6((void *)&NetNDRepPacketIP6, (void *)&NetNDSolPacketIP6);
	}
	
	ip6_send_ns ( &NetNDRepPacketIP6 );

}

void
ip6_NDISC_TimeoutCheck (void)
{
	ulong t;

	if (NetCompareIP6((void *)&NetNDSolPacketIP6, (void *)&NetNullAddrIP6) == 0)
		return;

	t = get_timer(0);

	/* check for NDISC timeout */
	if ((t - NetNDTimerStart) > NDISC_TIMEOUT * CONFIG_SYS_HZ) 
	{
		NetNDTry++;
		if (NetNDTry >= NDISC_TIMEOUT_COUNT) 
		{
			puts ("\nNeighbour discovery retry count exceeded; starting again\n");
			NetNDTry = 0;
			NetStartAgain();
		} 
		else 
		{
			NetNDTimerStart = t;
			ip6_NDISC_Request();
		}
	}
}

void
ip6_NDISC_init (void)
{
	/* XXX problem with bss workaround */
	NetNDPacketMAC = NULL;
	NetNDTxPacket = NULL;
	NetCopyIP6((void *)&NetNDSolPacketIP6, (void *)&NetNullAddrIP6);
	NetCopyIP6((void *)&NetNDRepPacketIP6, (void *)&NetNullAddrIP6);
	NetNDTxPacket = NULL;
	NetTxPacket = NULL;

	ip6_make_lladdr ((uchar *)&NetOurLinkLocalIP6, NetOurEther);
	
	if (!NetTxPacket) {
		int	i;
		/*
		 *	Setup packet buffers, aligned correctly.
		 */
		NetTxPacket = &PktBuf[0] + (PKTALIGN - 1);
		NetTxPacket -= (ulong)NetTxPacket % PKTALIGN;
		for (i = 0; i < PKTBUFSRX; i++) {
			NetRxPackets[i] = NetTxPacket + (i+1)*PKTSIZE_ALIGN;
		}
	}

	if (!NetNDTxPacket) {
		NetNDTxPacket = &NetArpWaitPacketBuf[0] + (PKTALIGN - 1);
		NetNDTxPacket -= (ulong)NetNDTxPacket % PKTALIGN;
		NetNDTxPacketSize = 0;
	}
}

int
NetSendUDPPacket6 (uchar *ether, IP6addr_t *dest, int dport, int sport, int len)
{
	uchar *pkt;
	UDP_t *udp;

	udp = (UDP_t *)((uchar *)NetTxPacket + NetEthHdrSize() + IP6_HDR_SIZE);

	udp->udp_dst = dport;
	udp->udp_src = sport;
	udp->udp_len = len + IP6_UDPHDR_SIZE;
	/* checksum */
	udp->udp_xsum = 0;
	udp->udp_xsum = csum_ipv6_magic(&NetOurIP6,	dest, len + IP6_UDPHDR_SIZE,
		IPPROTO_UDP, csum_partial((__u8 *) udp, len + IP6_UDPHDR_SIZE, 0));
	 
	/* if MAC address was not discovered yet, save the packet and do neighbour discovery */
	if (memcmp(ether, NetEtherNullAddr, 6) == 0) 
	{

		NetCopyIP6((void *)&NetNDSolPacketIP6, (void *)dest);
		NetNDPacketMAC = ether;

		pkt = NetNDTxPacket;
		pkt += NetSetEther (pkt, NetNDPacketMAC, PROT_IP6);
		pkt += ip6_add_hdr (pkt, &NetOurIP6, dest, IPPROTO_UDP, 64, len + IP6_UDPHDR_SIZE); 
		memcpy(pkt, (uchar *)udp, len + IP6_UDPHDR_SIZE);

		/* size of the waiting packet */
		NetNDTxPacketSize = (pkt - NetNDTxPacket) + IP6_UDPHDR_SIZE + len;

		/* and do the neighbor solicitation */
		NetNDTry = 1;
		NetNDTimerStart = get_timer(0);
		ip6_NDISC_Request();
		return 1;	/* waiting */
	}

	pkt = (uchar *)NetTxPacket;
	pkt += NetSetEther (pkt, ether, PROT_IP6);
	pkt += ip6_add_hdr (pkt, &NetOurIP6, dest, IPPROTO_UDP, 64, len + IP6_UDPHDR_SIZE);
	(void) eth_send(NetTxPacket, (pkt - NetTxPacket) + IP6_UDPHDR_SIZE + len);

	return 0;	/* transmitted */
}


#if defined(CONFIG_CMD_PING)

int PingSend6 (void)
{
	uchar *pkt;
	static uchar mac[6];

	/* always send arp request */
	memcpy(mac, NetEtherNullAddr, 6);

	NetCopyIP6((void *)&NetNDSolPacketIP6, (void *)&NetPingIP6);
	NetNDPacketMAC = mac;

	pkt = NetNDTxPacket;
	pkt += ip6_make_ping ( mac, &NetPingIP6, pkt );

	/* size of the waiting packet */
	NetNDTxPacketSize = (pkt - NetNDTxPacket);

	/* and do the ARP request */
	NetNDTry = 1;
	NetNDTimerStart = get_timer(0);
	ip6_NDISC_Request();
	return 1;	/* waiting */
}

static void
PingTimeout6 (void)
{
	eth_halt();
	NetState = NETLOOP_FAIL;	/* we did not get the reply */
}

static void
PingHandler6 (uchar * pkt, unsigned dest, unsigned src, unsigned len)
{
	IP6addr_t tmp;
	volatile IP6_t *ip6 = (volatile IP6_t *)pkt;

	NetReadIP6((void *)&ip6->saddr, &tmp);
	if (NetCompareIP6((void *)&NetPingIP6, (void *)&tmp) != 0)
		return;

	NetState = NETLOOP_SUCCESS;
}

void
PingStart6 (void)
{
#if defined(CONFIG_NET_MULTI)
	printf ("Using %s device\n", eth_get_name());
#endif	/* CONFIG_NET_MULTI */
	NetSetTimeout (10 * CONFIG_SYS_HZ, PingTimeout6);
	NetSetHandler (PingHandler6);

	PingSend6();
}
#endif	/* CFG_CMD_PING */


void
NetIP6PacketHandler (IP6_t *ip6, int len)
{
ICMP6_t			*icmp;
UDP_t				*udp;
__u16				csum;
NDISC_ND_t	*ndisc;
struct nd_opt_hdr	*opt;
uchar				neigh_eth_addr[6];


#ifdef ET_DEBUG
//	debug_printf ("IPv6 packet is received\n");
#endif	/* ET_DEBUG */

	if ( len < IP6_HDR_SIZE )
	{
#ifdef ET_DEBUG
		debug_printf ("IPv6 packet length is less than header length\n");
#endif	/* ET_DEBUG */
		return;
	}

	if ( ip6->version != 6 )
	{
#ifdef ET_DEBUG
		debug_printf ("Version is not 6\n");
#endif	/* ET_DEBUG */
		return;
	}

	switch ( ip6->nexthdr )
	{
		case IPPROTO_ICMPV6:
			icmp = (ICMP6_t *)(((uchar *)ip6) + IP6_HDR_SIZE);
			csum = icmp->icmp6_cksum;
			icmp->icmp6_cksum = 0;
			/* checksum */
			icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr, ip6->payload_len,
				IPPROTO_ICMPV6, csum_partial((__u8 *) icmp, ip6->payload_len, 0));
#ifdef ET_DEBUG
			debug_printf ("Receives ICMPv6 typr=%d\n", icmp->icmp6_type);
#endif	/* ET_DEBUG */
			if ( icmp->icmp6_cksum != csum )
			{
#ifdef ET_DEBUG
				debug_printf ("Incorrect ICMP checksum\n");
#endif	/* ET_DEBU */
				return;
			}
			switch ( icmp->icmp6_type )
			{
#if defined (CONFIG_CMD_PING)
				case IPV6_ICMP_ECHO_REQUEST:
					// ignore for now....
				break;

				case IPV6_ICMP_ECHO_REPLY:
					(*packetHandler)((uchar *)ip6, 0, 0, 0);
					return;
					break;
#endif	/* (CONFIG_CMD_PING) */

				case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
					ndisc = (NDISC_ND_t *)icmp;
					opt = (struct nd_opt_hdr	*)ndisc->opt;
					if ( ((NetCompareIP6((void *)&ndisc->target, (void *)&NetOurIP6) ==	0) ||
								(NetCompareIP6((void *)&ndisc->target, (void *)&NetOurLinkLocalIP6) ==	0)) &&
							 (ip6->payload_len > sizeof(ICMP6_t)) &&
							 ip6_ndisc_extract_addr_option ((u8 *)opt, ND_OPT_SOURCE_LL_ADDR, (void *)neigh_eth_addr) )
					{
#ifdef ET_DEBUG
						debug_printf ("Receive solicit for ");
						printf_IP6addr(&ndisc->target); debug_printf (" from ");
						printf_IP6addr(&ip6->saddr); debug_printf ("\n"); 
#endif	/* ET_DEBUG */
						ip6_send_na (neigh_eth_addr, &ip6->saddr, &ndisc->target);
					}
#ifdef ET_DEBUG
					else
					{
						if ( (NetCompareIP6((void *)&ndisc->target, (void *)&NetOurIP6) !=	0) &&
						     (NetCompareIP6((void *)&ndisc->target, (void *)&NetOurLinkLocalIP6) !=	0) )
						{
							debug_printf ("target ["); printf_IP6addr(&ndisc->target);
							debug_printf ("] not equal to our's ["); printf_IP6addr(&NetOurIP6); debug_printf ("]\n");   
						}
						if ( ip6->payload_len <= sizeof(ICMP6_t) )
							debug_printf ("ip6 payload size=%d <= sizeof(ICMPv6)[%d]\n", ip6->payload_len, sizeof(ICMP6_t));
						if ( !ip6_ndisc_extract_addr_option ((u8 *)opt, ND_OPT_SOURCE_LL_ADDR, (void *)neigh_eth_addr) )
							debug_printf ("No valid SLL option ");
					}
#endif /* ET_DEBUG */
					break;

				case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
					/* are we waiting for a reply ? */
					if ((NetCompareIP6((void *)&NetNDSolPacketIP6, (void *)&NetNullAddrIP6) ==	0) || !NetNDPacketMAC)
						break;

					ndisc = (NDISC_ND_t *)icmp;
					opt = (struct nd_opt_hdr	*)ndisc->opt;
					if ( (NetCompareIP6((void *)&ndisc->target, (void *)&NetNDRepPacketIP6) ==	0) &&
							 (ip6->payload_len > sizeof(ICMP6_t)) &&
							 ip6_ndisc_extract_addr_option ((u8 *)opt, ND_OPT_TARGET_LL_ADDR, (void *)neigh_eth_addr) )
					{
						memcpy (NetNDPacketMAC, neigh_eth_addr, 6);
						memcpy (NetServerEther, neigh_eth_addr, 6);
#ifdef ET_DEBUG
						debug_printf ("Receive advertisement for ");
						printf_IP6addr(&ndisc->target); debug_printf (" from ");
						printf_IP6addr(&ip6->saddr); debug_printf ("\n"); 
#endif	/* ET_DEBUG */
					}
					else
					{
#ifdef ET_DEBUG
						debug_printf ("Incorrect NA - ignored\n");
						debug_printf ("["); printf_IP6addr(&NetNDRepPacketIP6); debug_printf ("] "); printf_IP6addr(&ndisc->target); debug_printf ("\n"); 
						debug_printf ("[%d] %d\n", sizeof(ICMP6_t), ip6->payload_len);
						debug_printf ("%02X:%02X:%02X:%02X:%02X:%02X - ignored\n", neigh_eth_addr[0], neigh_eth_addr[1],
							neigh_eth_addr[2], neigh_eth_addr[3], neigh_eth_addr[4], neigh_eth_addr[5]);
#endif	/* ET_DEBUG */
						break;
					}

#ifdef CONFIG_NETCONSOLE
					(*packetHandler)(0,0,0,0);
#endif /* CONFIG_NETCONSOLE */
					/* modify header, and transmit it */
					memcpy(((Ethernet_t *)NetNDTxPacket)->et_dest, NetNDPacketMAC, 6);
					(void) eth_send(NetNDTxPacket, NetNDTxPacketSize);

					/* no ND request pending now */
					NetCopyIP6((void *)&NetNDSolPacketIP6, (void *)&NetNullAddrIP6);
					NetNDTxPacketSize = 0;
					NetNDPacketMAC = NULL;
					break;

				default:
#ifdef ET_DEBUG
					debug_printf ("Ignored - unrecognised ICMP type %d\n", icmp->icmp6_type);
#endif	/* ET_DEBUG */
				return;
				break;
			}
			break;

		case IPPROTO_UDP:
			udp = (UDP_t *)(((uchar *)ip6) + IP6_HDR_SIZE);
			/* check udp checksum - TODO */
			csum = udp->udp_xsum;
			udp->udp_xsum = 0;
			/* checksum */
			udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr, ip6->payload_len,
				IPPROTO_UDP, csum_partial((__u8 *) udp, ip6->payload_len, 0));
			if ( csum != udp->udp_xsum )
			{
#ifdef ET_DEBUG
				debug_printf ("Incorrect UDP checksum[0x%04X] 0x%04X\n", udp->udp_xsum, csum);
#endif	/* ET_DEBUG */
				return;
			}
			/* handle the packet */
#ifdef CONFIG_NETCONSOLE
			nc_input_packet((uchar *)ip6 + IP6_HDR_SIZE + IP6_UDPHDR_SIZE,
						ntohs(udp->udp_dst),
						ntohs(udp->udp_src),
						ntohs(udp->udp_len) - 8);
#endif
		/* IP header OK.  Pass the packet to the current handler.
		 */
			(*packetHandler)((uchar *)ip6 + IP6_HDR_SIZE + IP6_UDPHDR_SIZE,
						ntohs(udp->udp_dst),
						ntohs(udp->udp_src),
						ntohs(udp->udp_len) - 8);
			break;

		default:
#ifdef ET_DEBUG
				debug_printf ("Ignored - unrecognised nextheader %d\n", ip6->nexthdr);
#endif	/* ET_DEBU */
			return;
			break;
	}

}

char *
ip6_to_string (const u_int8_t * src, int8_t * dst, size_t size)
{
  int i;

  struct {
    int base, len;
  } best, cur;

  char tmp[IP6_ADDR_MAXSTRLEN], *tp;
  unsigned int words[IN6ADDRSZ / INT16SZ];
 
  if ( (src == NULL) || (dst == NULL) )
		return( (char *)dst );
  
  /*
   * Preprocess:
   *      Copy the input (bytewise) array into a wordwise array.
   *      Find the longest run of 0x00's in src[] for :: shorthanding.
   */
  memset(words, '\0', sizeof(words));

  for ( i = 0; i < IN6ADDRSZ; i++ )
    words[i / 2] |= (src[i] << ((1 - (i % 2)) << 3));

  best.base = -1;
  cur.base = -1;
  best.len = 0;
  cur.len = 0;
  for ( i = 0; i < (IN6ADDRSZ / INT16SZ); i++ ) 
  {
    if ( words[i] == 0 ) 
    {
      if ( cur.base == -1 )
        cur.base = i, cur.len = 1;
      else
        cur.len++;
    }  
    else 
    {
      if ( cur.base != -1 ) 
      {
        if ( best.base == -1 || cur.len > best.len )
          best = cur;
        cur.base = -1;
      }
    }
  }
  if ( cur.base != -1 ) 
  {
    if ( best.base == -1 || cur.len > best.len )
      best = cur;
  }
  if ( best.base != -1 && best.len < 2 )
    best.base = -1;

  /*
   * Format the result.
   */
  tp = tmp;
  for ( i = 0; i < (IN6ADDRSZ / INT16SZ); i++ ) 
  {
    /* Are we inside the best run of 0x00's? */
    if ( best.base != -1 && i >= best.base &&
         i < (best.base + best.len) ) 
    {
      if ( i == best.base )
        *tp++ = ':';
      continue;
    }
    /* Are we following an initial run of 0x00s or any real hex? */
    if ( i != 0 )
      *tp++ = ':';
    /* Is this address an encapsulated IPv4? */
    if ( i == 6 && best.base == 0 &&
         (best.len == 6 || (best.len == 5 && words[5] == 0xffff)) ) 
    {
      ip_to_string((IPaddr_t)src+12, tp);
      tp += strlen(tp);
      break;
    }
    tp += sprintf(tp, "%x", words[i]);
  }
  /* Was it a trailing run of 0x00's? */
  if ( best.base != -1 && (best.base + best.len) ==
       (IN6ADDRSZ / INT16SZ) )
    *tp++ = ':';
  *tp++ = '\0';
 
  /*
   * Check for overflow, copy, and we're done.
   */
  if ( (size_t)(tp - tmp) > size ) 
  {
    return NULL;
  }
  strcpy((char *)dst, tmp);
  return (char *)dst;
}

int
string_to_ip6 (const char *strptr, void *addrptr)
{
  IP6addr_t *in6_val = (IP6addr_t *) addrptr;
  int                 colon_count = 0;
  int            	  found_double_colon = 0;
  int                 xstart = 0;  /* first zero (double colon) */
  int                 len = 7;     /* numbers of zero words the double colon represents */
  int                 i;
  const char      *s = strptr;
  
  if ( strptr == NULL )
		return 0;
  
  /* First pass, verify the syntax and locate the double colon if present */
  for (;;)
  {
      while (isxdigit((int)*s))
          s++;
      if (*s == '\0')
          break;
      if (*s != ':')
      {
          if (*s == '.' && len >= 2)
          {
              while (s != strptr && *(s-1) != ':')
                  --s;
              if (inet_pton4(s, in6_val) == 1)
              {
                  len -= 2;
                  break;
              }
          }
          /* This could be a valid address */
          break;
      }
      if (s == strptr)
      {
          /* The address begins with a colon */
          if (*++s != ':')
              /* Must start with a double colon or a number */
              goto errout1;
      }
      else
      {
          s++;
          if (found_double_colon)
              len--;
          else
              xstart++;
      }
      
      if (*s == ':')
      {
          if (found_double_colon)
              /* Two double colons are not allowed */
              goto errout1;
          found_double_colon = 1;
          len -= xstart;
          s++;
      }
      
      if (++colon_count == 7)
          /* Found all colons */
          break;
  }
  
  if (colon_count == 0 || colon_count > 7)
      goto errout1;
  if (*--s == ':')
      len++;
  
  /* Second pass, read the address */
  s = strptr;
  for (i = 0; i < 8; i++)
  {
      int val = 0;
      
      if (found_double_colon && i >= xstart && i < xstart + len)
      {
          ((u_int16_t *)in6_val)[i] = 0;
          continue;
      }
      while (*s == ':')
          s++;

      if (i == 6 && isdigit((int)*s)
          && inet_pton4(s, &(((u_int16_t *)in6_val)[i])) == 1)
          /* Ending with :IPv4-address, which is valid */
          break;

      while (isxdigit((int)*s))
      {
          val <<= 4;
          if (*s >= '0' && *s <= '9')
              val += (*s - '0');
          else if (*s >= 'A' && *s <= 'F')
              val += (*s - 'A' + 10);
          else
              val += (*s - 'a' + 10);
          s++;
      }
      ((u_int16_t *)in6_val)[i] = (u_int16_t)htons(val);
  }
  return 1;

errout1:
  return 0;
}

void
print_IP6addr (IP6addr_t *x)
{
	char tmp[(IP6_ADDR_MAXSTRLEN)*2];

	ip6_to_string ((u_int8_t *)x, (int8_t *)tmp, IP6_ADDR_MAXSTRLEN*2);

	puts (tmp);
}

void
printf_IP6addr (IP6addr_t *x)
{
	char tmp[(IP6_ADDR_MAXSTRLEN)*2];

	ip6_to_string ((u_int8_t *)x, (int8_t *)tmp, IP6_ADDR_MAXSTRLEN*2);

#ifdef ET_DEBUG
	debug_printf ("%s", tmp);
#else
	printf ("%s", tmp);
#endif
}

int
getenv_IP6addr (char *var, IP6addr_t *ip6addr)
{
	if ( getenv(var) )
	{
		return (string_to_ip6(getenv(var), (void *)ip6addr));
	}

	return( 0 );

}

int
inet_pton4 (const char *strptr, void *addrptr)
{
  int    octet;
  int    octval;
  u_int32_t addr = 0;
  
  for (octet = 1; octet <= 4; octet++)
  {
      octval = 0;
      while (isdigit((int)*strptr))
          octval = octval * 10 + (*strptr++ - '0');
      if (octval > 255 || (*strptr++ != '.' && octet != 4))
          goto errout;
      
      addr = (addr << 8) + octval;
  }
  IP_SET_HTONL(addrptr, addr);
  return 1;

errout:
  return 0;
}


#ifdef ET_DEBUG
void
debug_printf (const char *fmt, ...)
{
	va_list args;
	uint i;
	char printbuffer[CONFIG_SYS_PBSIZE];

	va_start (args, fmt);

	/* For this to work, printbuffer must be larger than
	* anything we ever want to print.
	*/
	i = vsprintf (printbuffer, fmt, args);
	va_end (args);

	/* Print the string */
	serial_puts (printbuffer);
}


void
ip6_print_env (void)
{
	debug_printf("env(%s)=%s  val=%d\n", "ipversion", getenv("ipversion") ? getenv("ipversion"):"4", NetIpVersion);
	debug_printf("env(%s)=%s  val=", "ipaddr6", getenv("ipaddr6") ? getenv("ipaddr6") : ""); printf_IP6addr(&NetOurIP6); debug_printf("\n"); 
	debug_printf("env(%s)=%s  val=%d\n", "prefixlength6", getenv("prefixlength6") ? getenv("prefixlength6") : "", NetPrefixLength);
	debug_printf("env(%s)=%s  val=", "gatewayip6", getenv("gatewayip6") ? getenv("gatewayip6") : "" ); printf_IP6addr(&NetOurGatewayIP6); debug_printf("\n");
	debug_printf("env(%s)=%s  val=", "serverip6", getenv("serverip6") ? getenv("serverip6") : ""); printf_IP6addr(&NetServerIP6); debug_printf("\n");
}

#endif


#endif
