net/net6.c - mirrors/cros/chromiumos/third_party/u-boot - Git at Google

 // SPDX-License-Identifier: GPL-2.0+
 /*
  * Copyright (C) 2013 Allied Telesis Labs NZ
  * Chris Packham, <judge.packham@gmail.com>
  *
  * Copyright (C) 2022 YADRO
  * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
  */

 /* Simple IPv6 network layer implementation */

 #include <common.h>
 #include <env_internal.h>
 #include <malloc.h>
 #include <net.h>
 #include <net6.h>
 #include <ndisc.h>

 /* NULL IPv6 address */
 struct in6_addr const net_null_addr_ip6 = ZERO_IPV6_ADDR;
 /* Our gateway's IPv6 address */
 struct in6_addr net_gateway6 = ZERO_IPV6_ADDR;
 /* Our IPv6 addr (0 = unknown) */
 struct in6_addr net_ip6 = ZERO_IPV6_ADDR;
 /* Our link local IPv6 addr (0 = unknown) */
 struct in6_addr net_link_local_ip6 = ZERO_IPV6_ADDR;
 /* set server IPv6 addr (0 = unknown) */
 struct in6_addr net_server_ip6 = ZERO_IPV6_ADDR;
 /* The prefix length of our network */
 u32 net_prefix_length;

 bool use_ip6;

 static int on_ip6addr(const char *name, const char *value, enum env_op op,
 		      int flags)
 {
 	char *mask;
 	size_t len;

 	if (flags & H_PROGRAMMATIC)
 		return 0;

 	if (op == env_op_delete) {
 		net_prefix_length = 0;
 		net_copy_ip6(&net_ip6, &net_null_addr_ip6);
 		return 0;
 	}

 	mask = strchr(value, '/');
 	len = strlen(value);

 	if (mask)
 		net_prefix_length = simple_strtoul(value + len, NULL, 10);

 	return string_to_ip6(value, len, &net_ip6);
 }

 U_BOOT_ENV_CALLBACK(ip6addr, on_ip6addr);

 static int on_gatewayip6(const char *name, const char *value, enum env_op op,
 			 int flags)
 {
 	if (flags & H_PROGRAMMATIC)
 		return 0;

 	return string_to_ip6(value, strlen(value), &net_gateway6);
 }

 U_BOOT_ENV_CALLBACK(gatewayip6, on_gatewayip6);

 static int on_serverip6(const char *name, const char *value, enum env_op op,
 			int flags)
 {
 	if (flags & H_PROGRAMMATIC)
 		return 0;

 	return string_to_ip6(value, strlen(value), &net_server_ip6);
 }

 U_BOOT_ENV_CALLBACK(serverip6, on_serverip6);

 int ip6_is_unspecified_addr(struct in6_addr *addr)
 {
 	return !(addr->s6_addr32[0] | addr->s6_addr32[1] |
 		addr->s6_addr32[2] | addr->s6_addr32[3]);
 }

 int ip6_is_our_addr(struct in6_addr *addr)
 {
 	return !memcmp(addr, &net_link_local_ip6, sizeof(struct in6_addr)) ||
 	       !memcmp(addr, &net_ip6, sizeof(struct in6_addr));
 }

 void ip6_make_eui(unsigned char eui[8], unsigned char const enetaddr[6])
 {
 	memcpy(eui, enetaddr, 3);
 	memcpy(&eui[5], &enetaddr[3], 3);
 	eui[3] = 0xff;
 	eui[4] = 0xfe;
 	eui[0] ^= 2;		/* "u" bit set to indicate global scope */
 }

 void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6])
 {
 	unsigned char eui[8];

 	memset(lladr, 0, sizeof(struct in6_addr));
 	lladr->s6_addr16[0] = htons(IPV6_LINK_LOCAL_PREFIX);
 	ip6_make_eui(eui, enetaddr);
 	memcpy(&lladr->s6_addr[8], eui, 8);
 }

 void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
 {
 	memset(mcast_addr, 0, sizeof(struct in6_addr));
 	mcast_addr->s6_addr[0] = 0xff;
 	mcast_addr->s6_addr[1] = IPV6_ADDRSCOPE_LINK;
 	mcast_addr->s6_addr[11] = 0x01;
 	mcast_addr->s6_addr[12] = 0xff;
 	mcast_addr->s6_addr[13] = ip6_addr->s6_addr[13];
 	mcast_addr->s6_addr[14] = ip6_addr->s6_addr[14];
 	mcast_addr->s6_addr[15] = ip6_addr->s6_addr[15];
 }

 void
 ip6_make_mult_ethdstaddr(unsigned char enetaddr[6], struct in6_addr *mcast_addr)
 {
 	enetaddr[0] = 0x33;
 	enetaddr[1] = 0x33;
 	memcpy(&enetaddr[2], &mcast_addr->s6_addr[12], 4);
 }

 int
 ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
 		   u32 plen)
 {
 	__be32 *addr_dwords;
 	__be32 *neigh_dwords;

 	addr_dwords = our_addr->s6_addr32;
 	neigh_dwords = neigh_addr->s6_addr32;

 	while (plen > 32) {
 		if (*addr_dwords++ != *neigh_dwords++)
 			return 0;

 		plen -= 32;
 	}

 	/* Check any remaining bits */
 	if (plen > 0) {
 		if ((*addr_dwords >> (32 - plen)) !=
 		    (*neigh_dwords >> (32 - plen))) {
 			return 0;
 		}
 	}

 	return 1;
 }

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

 	/* Opaque moment. If reverse it to zero it will not be checked on
 	 * receiver's side. It leads to bad negibour advertisement.
 	 */
 	if (sum == 0xffff)
 		return sum;

 	return ~sum;
 }

 static inline unsigned short from32to16(unsigned int x)
 {
 	/* add up 16-bit and 16-bit for 16+c bit */
 	x = (x & 0xffff) + (x >> 16);
 	/* add up carry.. */
 	x = (x & 0xffff) + (x >> 16);
 	return x;
 }

 static u32 csum_do_csum(const u8 *buff, int len)
 {
 	int odd;
 	unsigned int result = 0;

 	if (len <= 0)
 		goto out;
 	odd = 1 & (unsigned long)buff;
 	if (odd) {
 #ifdef __LITTLE_ENDIAN
 		result += (*buff << 8);
 #else
 		result = *buff;
 #endif
 		len--;
 		buff++;
 	}
 	if (len >= 2) {
 		if (2 & (unsigned long)buff) {
 			result += *(unsigned short *)buff;
 			len -= 2;
 			buff += 2;
 		}
 		if (len >= 4) {
 			const unsigned char *end = buff + ((u32)len & ~3);
 			unsigned int carry = 0;

 			do {
 				unsigned int w = *(unsigned int *)buff;

 				buff += 4;
 				result += carry;
 				result += w;
 				carry = (w > result);
 			} while (buff < end);
 			result += carry;
 			result = (result & 0xffff) + (result >> 16);
 		}
 		if (len & 2) {
 			result += *(unsigned short *)buff;
 			buff += 2;
 		}
 	}
 	if (len & 1)
 #ifdef __LITTLE_ENDIAN
 		result += *buff;
 #else
 		result += (*buff << 8);
 #endif
 	result = from32to16(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;
 }

 unsigned short int
 csum_ipv6_magic(struct in6_addr *saddr, struct in6_addr *daddr, u16 len,
 		unsigned short proto, unsigned int csum)
 {
 	int carry;
 	u32 ulen;
 	u32 uproto;
 	u32 sum = csum;

 	sum += saddr->s6_addr32[0];
 	carry = (sum < saddr->s6_addr32[0]);
 	sum += carry;

 	sum += saddr->s6_addr32[1];
 	carry = (sum < saddr->s6_addr32[1]);
 	sum += carry;

 	sum += saddr->s6_addr32[2];
 	carry = (sum < saddr->s6_addr32[2]);
 	sum += carry;

 	sum += saddr->s6_addr32[3];
 	carry = (sum < saddr->s6_addr32[3]);
 	sum += carry;

 	sum += daddr->s6_addr32[0];
 	carry = (sum < daddr->s6_addr32[0]);
 	sum += carry;

 	sum += daddr->s6_addr32[1];
 	carry = (sum < daddr->s6_addr32[1]);
 	sum += carry;

 	sum += daddr->s6_addr32[2];
 	carry = (sum < daddr->s6_addr32[2]);
 	sum += carry;

 	sum += daddr->s6_addr32[3];
 	carry = (sum < daddr->s6_addr32[3]);
 	sum += carry;

 	ulen = htonl((u32)len);
 	sum += ulen;
 	carry = (sum < ulen);
 	sum += carry;

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

 	return csum_fold(sum);
 }

 int ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
 		int nextheader, int hoplimit, int payload_len)
 {
 	struct ip6_hdr *ip6 = (struct ip6_hdr *)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;
 	net_copy_ip6(&ip6->saddr, src);
 	net_copy_ip6(&ip6->daddr, dest);

 	return sizeof(struct ip6_hdr);
 }

 int net_send_udp_packet6(uchar *ether, struct in6_addr *dest, int dport,
 			 int sport, int len)
 {
 	uchar *pkt;
 	struct udp_hdr *udp;
 	u16 csum_p;

 	udp = (struct udp_hdr *)((uchar *)net_tx_packet + net_eth_hdr_size() +
 			IP6_HDR_SIZE);

 	udp->udp_dst = htons(dport);
 	udp->udp_src = htons(sport);
 	udp->udp_len = htons(len + UDP_HDR_SIZE);

 	/* checksum */
 	udp->udp_xsum = 0;
 	csum_p = csum_partial((u8 *)udp, len + UDP_HDR_SIZE, 0);
 	udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
 					IPPROTO_UDP, csum_p);

 	/* if MAC address was not discovered yet, save the packet and do
 	 * neighbour discovery
 	 */
 	if (!memcmp(ether, net_null_ethaddr, 6)) {
 		net_copy_ip6(&net_nd_sol_packet_ip6, dest);
 		net_nd_packet_mac = ether;

 		pkt = net_nd_tx_packet;
 		pkt += net_set_ether(pkt, net_nd_packet_mac, PROT_IP6);
 		pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
 				len + UDP_HDR_SIZE);
 		memcpy(pkt, (uchar *)udp, len + UDP_HDR_SIZE);

 		/* size of the waiting packet */
 		net_nd_tx_packet_size = (pkt - net_nd_tx_packet) +
 			UDP_HDR_SIZE + len;

 		/* and do the neighbor solicitation */
 		net_nd_try = 1;
 		net_nd_timer_start = get_timer(0);
 		ndisc_request();
 		return 1;	/* waiting */
 	}

 	pkt = (uchar *)net_tx_packet;
 	pkt += net_set_ether(pkt, ether, PROT_IP6);
 	pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
 			len + UDP_HDR_SIZE);
 	(void)eth_send(net_tx_packet, pkt - net_tx_packet + UDP_HDR_SIZE + len);

 	return 0;	/* transmitted */
 }

 int net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 {
 	struct in_addr zero_ip = {.s_addr = 0 };
 	struct icmp6hdr *icmp;
 	struct udp_hdr *udp;
 	u16 csum;
 	u16 csum_p;
 	u16 hlen;

 	if (len < IP6_HDR_SIZE)
 		return -EINVAL;

 	if (ip6->version != 6)
 		return -EINVAL;

 	switch (ip6->nexthdr) {
 	case PROT_ICMPV6:
 		icmp = (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
 		csum = icmp->icmp6_cksum;
 		hlen = ntohs(ip6->payload_len);
 		icmp->icmp6_cksum = 0;
 		/* checksum */
 		csum_p = csum_partial((u8 *)icmp, hlen, 0);
 		icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
 						    hlen, PROT_ICMPV6, csum_p);

 		if (icmp->icmp6_cksum != csum)
 			return -EINVAL;

 		switch (icmp->icmp6_type) {
 		case IPV6_ICMP_ECHO_REQUEST:
 		case IPV6_ICMP_ECHO_REPLY:
 			ping6_receive(et, ip6, len);
 			break;
 		case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
 		case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
 			ndisc_receive(et, ip6, len);
 			break;
 		default:
 			break;
 		}
 		break;
 	case IPPROTO_UDP:
 		udp = (struct udp_hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
 		csum = udp->udp_xsum;
 		hlen = ntohs(ip6->payload_len);
 		udp->udp_xsum = 0;
 		/* checksum */
 		csum_p = csum_partial((u8 *)udp, hlen, 0);
 		udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
 						hlen, IPPROTO_UDP, csum_p);

 		if (csum != udp->udp_xsum)
 			return -EINVAL;

 		/* IP header OK. Pass the packet to the current handler. */
 		net_get_udp_handler()((uchar *)ip6 + IP6_HDR_SIZE +
 					UDP_HDR_SIZE,
 				ntohs(udp->udp_dst),
 				zero_ip,
 				ntohs(udp->udp_src),
 				ntohs(udp->udp_len) - 8);
 		break;
 	default:
 		return -EINVAL;
 	}

 	return 0;
 }
	// SPDX-License-Identifier: GPL-2.0+
	/*
	* Copyright (C) 2013 Allied Telesis Labs NZ
	* Chris Packham, <judge.packham@gmail.com>
	*
	* Copyright (C) 2022 YADRO
	* Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
	*/

	/* Simple IPv6 network layer implementation */

	#include <common.h>
	#include <env_internal.h>
	#include <malloc.h>
	#include <net.h>
	#include <net6.h>
	#include <ndisc.h>

	/* NULL IPv6 address */
	struct in6_addr const net_null_addr_ip6 = ZERO_IPV6_ADDR;
	/* Our gateway's IPv6 address */
	struct in6_addr net_gateway6 = ZERO_IPV6_ADDR;
	/* Our IPv6 addr (0 = unknown) */
	struct in6_addr net_ip6 = ZERO_IPV6_ADDR;
	/* Our link local IPv6 addr (0 = unknown) */
	struct in6_addr net_link_local_ip6 = ZERO_IPV6_ADDR;
	/* set server IPv6 addr (0 = unknown) */
	struct in6_addr net_server_ip6 = ZERO_IPV6_ADDR;
	/* The prefix length of our network */
	u32 net_prefix_length;

	bool use_ip6;

	static int on_ip6addr(const char name, const char value, enum env_op op,
	int flags)
	{
	char *mask;
	size_t len;

	if (flags & H_PROGRAMMATIC)
	return 0;

	if (op == env_op_delete) {
	net_prefix_length = 0;
	net_copy_ip6(&net_ip6, &net_null_addr_ip6);
	return 0;
	}

	mask = strchr(value, '/');
	len = strlen(value);

	if (mask)
	net_prefix_length = simple_strtoul(value + len, NULL, 10);

	return string_to_ip6(value, len, &net_ip6);
	}

	U_BOOT_ENV_CALLBACK(ip6addr, on_ip6addr);

	static int on_gatewayip6(const char name, const char value, enum env_op op,
	int flags)
	{
	if (flags & H_PROGRAMMATIC)
	return 0;

	return string_to_ip6(value, strlen(value), &net_gateway6);
	}

	U_BOOT_ENV_CALLBACK(gatewayip6, on_gatewayip6);

	static int on_serverip6(const char name, const char value, enum env_op op,
	int flags)
	{
	if (flags & H_PROGRAMMATIC)
	return 0;

	return string_to_ip6(value, strlen(value), &net_server_ip6);
	}

	U_BOOT_ENV_CALLBACK(serverip6, on_serverip6);

	int ip6_is_unspecified_addr(struct in6_addr *addr)
	{
	return !(addr->s6_addr32[0] \| addr->s6_addr32[1] \|
	addr->s6_addr32[2] \| addr->s6_addr32[3]);
	}

	int ip6_is_our_addr(struct in6_addr *addr)
	{
	return !memcmp(addr, &net_link_local_ip6, sizeof(struct in6_addr)) \|\|
	!memcmp(addr, &net_ip6, sizeof(struct in6_addr));
	}

	void ip6_make_eui(unsigned char eui[8], unsigned char const enetaddr[6])
	{
	memcpy(eui, enetaddr, 3);
	memcpy(&eui[5], &enetaddr[3], 3);
	eui[3] = 0xff;
	eui[4] = 0xfe;
	eui[0] ^= 2; /* "u" bit set to indicate global scope */
	}

	void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6])
	{
	unsigned char eui[8];

	memset(lladr, 0, sizeof(struct in6_addr));
	lladr->s6_addr16[0] = htons(IPV6_LINK_LOCAL_PREFIX);
	ip6_make_eui(eui, enetaddr);
	memcpy(&lladr->s6_addr[8], eui, 8);
	}

	void ip6_make_snma(struct in6_addr mcast_addr, struct in6_addr ip6_addr)
	{
	memset(mcast_addr, 0, sizeof(struct in6_addr));
	mcast_addr->s6_addr[0] = 0xff;
	mcast_addr->s6_addr[1] = IPV6_ADDRSCOPE_LINK;
	mcast_addr->s6_addr[11] = 0x01;
	mcast_addr->s6_addr[12] = 0xff;
	mcast_addr->s6_addr[13] = ip6_addr->s6_addr[13];
	mcast_addr->s6_addr[14] = ip6_addr->s6_addr[14];
	mcast_addr->s6_addr[15] = ip6_addr->s6_addr[15];
	}

	void
	ip6_make_mult_ethdstaddr(unsigned char enetaddr[6], struct in6_addr *mcast_addr)
	{
	enetaddr[0] = 0x33;
	enetaddr[1] = 0x33;
	memcpy(&enetaddr[2], &mcast_addr->s6_addr[12], 4);
	}

	int
	ip6_addr_in_subnet(struct in6_addr our_addr, struct in6_addr neigh_addr,
	u32 plen)
	{
	__be32 *addr_dwords;
	__be32 *neigh_dwords;

	addr_dwords = our_addr->s6_addr32;
	neigh_dwords = neigh_addr->s6_addr32;

	while (plen > 32) {
	if (addr_dwords++ != neigh_dwords++)
	return 0;

	plen -= 32;
	}

	/* Check any remaining bits */
	if (plen > 0) {
	if ((*addr_dwords >> (32 - plen)) !=
	(*neigh_dwords >> (32 - plen))) {
	return 0;
	}
	}

	return 1;
	}

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

	/* Opaque moment. If reverse it to zero it will not be checked on
	* receiver's side. It leads to bad negibour advertisement.
	*/
	if (sum == 0xffff)
	return sum;

	return ~sum;
	}

	static inline unsigned short from32to16(unsigned int x)
	{
	/* add up 16-bit and 16-bit for 16+c bit */
	x = (x & 0xffff) + (x >> 16);
	/* add up carry.. */
	x = (x & 0xffff) + (x >> 16);
	return x;
	}

	static u32 csum_do_csum(const u8 *buff, int len)
	{
	int odd;
	unsigned int result = 0;

	if (len <= 0)
	goto out;
	odd = 1 & (unsigned long)buff;
	if (odd) {
	#ifdef __LITTLE_ENDIAN
	result += (*buff << 8);
	#else
	result = *buff;
	#endif
	len--;
	buff++;
	}
	if (len >= 2) {
	if (2 & (unsigned long)buff) {
	result += (unsigned short )buff;
	len -= 2;
	buff += 2;
	}
	if (len >= 4) {
	const unsigned char *end = buff + ((u32)len & ~3);
	unsigned int carry = 0;

	do {
	unsigned int w = (unsigned int )buff;

	buff += 4;
	result += carry;
	result += w;
	carry = (w > result);
	} while (buff < end);
	result += carry;
	result = (result & 0xffff) + (result >> 16);
	}
	if (len & 2) {
	result += (unsigned short )buff;
	buff += 2;
	}
	}
	if (len & 1)
	#ifdef __LITTLE_ENDIAN
	result += *buff;
	#else
	result += (*buff << 8);
	#endif
	result = from32to16(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;
	}

	unsigned short int
	csum_ipv6_magic(struct in6_addr saddr, struct in6_addr daddr, u16 len,
	unsigned short proto, unsigned int csum)
	{
	int carry;
	u32 ulen;
	u32 uproto;
	u32 sum = csum;

	sum += saddr->s6_addr32[0];
	carry = (sum < saddr->s6_addr32[0]);
	sum += carry;

	sum += saddr->s6_addr32[1];
	carry = (sum < saddr->s6_addr32[1]);
	sum += carry;

	sum += saddr->s6_addr32[2];
	carry = (sum < saddr->s6_addr32[2]);
	sum += carry;

	sum += saddr->s6_addr32[3];
	carry = (sum < saddr->s6_addr32[3]);
	sum += carry;

	sum += daddr->s6_addr32[0];
	carry = (sum < daddr->s6_addr32[0]);
	sum += carry;

	sum += daddr->s6_addr32[1];
	carry = (sum < daddr->s6_addr32[1]);
	sum += carry;

	sum += daddr->s6_addr32[2];
	carry = (sum < daddr->s6_addr32[2]);
	sum += carry;

	sum += daddr->s6_addr32[3];
	carry = (sum < daddr->s6_addr32[3]);
	sum += carry;

	ulen = htonl((u32)len);
	sum += ulen;
	carry = (sum < ulen);
	sum += carry;

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

	return csum_fold(sum);
	}

	int ip6_add_hdr(uchar xip, struct in6_addr src, struct in6_addr *dest,
	int nextheader, int hoplimit, int payload_len)
	{
	struct ip6_hdr ip6 = (struct ip6_hdr )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;
	net_copy_ip6(&ip6->saddr, src);
	net_copy_ip6(&ip6->daddr, dest);

	return sizeof(struct ip6_hdr);
	}

	int net_send_udp_packet6(uchar ether, struct in6_addr dest, int dport,
	int sport, int len)
	{
	uchar *pkt;
	struct udp_hdr *udp;
	u16 csum_p;

	udp = (struct udp_hdr )((uchar )net_tx_packet + net_eth_hdr_size() +
	IP6_HDR_SIZE);

	udp->udp_dst = htons(dport);
	udp->udp_src = htons(sport);
	udp->udp_len = htons(len + UDP_HDR_SIZE);

	/* checksum */
	udp->udp_xsum = 0;
	csum_p = csum_partial((u8 *)udp, len + UDP_HDR_SIZE, 0);
	udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
	IPPROTO_UDP, csum_p);

	/* if MAC address was not discovered yet, save the packet and do
	* neighbour discovery
	*/
	if (!memcmp(ether, net_null_ethaddr, 6)) {
	net_copy_ip6(&net_nd_sol_packet_ip6, dest);
	net_nd_packet_mac = ether;

	pkt = net_nd_tx_packet;
	pkt += net_set_ether(pkt, net_nd_packet_mac, PROT_IP6);
	pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
	len + UDP_HDR_SIZE);
	memcpy(pkt, (uchar *)udp, len + UDP_HDR_SIZE);

	/* size of the waiting packet */
	net_nd_tx_packet_size = (pkt - net_nd_tx_packet) +
	UDP_HDR_SIZE + len;

	/* and do the neighbor solicitation */
	net_nd_try = 1;
	net_nd_timer_start = get_timer(0);
	ndisc_request();
	return 1; /* waiting */
	}

	pkt = (uchar *)net_tx_packet;
	pkt += net_set_ether(pkt, ether, PROT_IP6);
	pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
	len + UDP_HDR_SIZE);
	(void)eth_send(net_tx_packet, pkt - net_tx_packet + UDP_HDR_SIZE + len);

	return 0; /* transmitted */
	}

	int net_ip6_handler(struct ethernet_hdr et, struct ip6_hdr ip6, int len)
	{
	struct in_addr zero_ip = {.s_addr = 0 };
	struct icmp6hdr *icmp;
	struct udp_hdr *udp;
	u16 csum;
	u16 csum_p;
	u16 hlen;

	if (len < IP6_HDR_SIZE)
	return -EINVAL;

	if (ip6->version != 6)
	return -EINVAL;

	switch (ip6->nexthdr) {
	case PROT_ICMPV6:
	icmp = (struct icmp6hdr )(((uchar )ip6) + IP6_HDR_SIZE);
	csum = icmp->icmp6_cksum;
	hlen = ntohs(ip6->payload_len);
	icmp->icmp6_cksum = 0;
	/* checksum */
	csum_p = csum_partial((u8 *)icmp, hlen, 0);
	icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
	hlen, PROT_ICMPV6, csum_p);

	if (icmp->icmp6_cksum != csum)
	return -EINVAL;

	switch (icmp->icmp6_type) {
	case IPV6_ICMP_ECHO_REQUEST:
	case IPV6_ICMP_ECHO_REPLY:
	ping6_receive(et, ip6, len);
	break;
	case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
	case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
	ndisc_receive(et, ip6, len);
	break;
	default:
	break;
	}
	break;
	case IPPROTO_UDP:
	udp = (struct udp_hdr )(((uchar )ip6) + IP6_HDR_SIZE);
	csum = udp->udp_xsum;
	hlen = ntohs(ip6->payload_len);
	udp->udp_xsum = 0;
	/* checksum */
	csum_p = csum_partial((u8 *)udp, hlen, 0);
	udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
	hlen, IPPROTO_UDP, csum_p);

	if (csum != udp->udp_xsum)
	return -EINVAL;

	/* IP header OK. Pass the packet to the current handler. */
	net_get_udp_handler()((uchar *)ip6 + IP6_HDR_SIZE +
	UDP_HDR_SIZE,
	ntohs(udp->udp_dst),
	zero_ip,
	ntohs(udp->udp_src),
	ntohs(udp->udp_len) - 8);
	break;
	default:
	return -EINVAL;
	}

	return 0;
	}