net: ipv6: Add implementation of main IPv6 functions

Functions that were exposed in "net: ipv6: Add IPv6 basic primitives"
had only empty implementations and were exposed as API for futher
patches. This patch add implementation of these functions. Main
functions are: net_ip6_handler() - IPv6 packet handler for incoming
packets; net_send_udp_packet6() - make up and send an UDP packet;
csum_ipv6_magic() - compute checksum of IPv6 "psuedo-header" per RFC2460
section 8.1; ip6_addr_in_subnet() - check if an address is in our
subnet. Other functions are auxiliary.

Series-changes: 3
- Added comments
- Fixed style problems
- Fixed return codes instead of -1

Signed-off-by: Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
Reviewed-by: Ramon Fried <rfried.dev@gmail.com>
Reviewed-by: Simon Glass <sjg@chromium.org>

2 files changed, 459 insertions, 64 deletions

diff --git a/include/net6.h b/include/net6.h
index 50b7120ca34..622885e7df8 100644
--- a/include/net6.h
+++ b/include/net6.h
@@ -195,13 +195,6 @@ extern bool use_ip6;
  * Return: 0 if conversion successful, -EINVAL if fail
  */
 int string_to_ip6(const char *s, size_t len, struct in6_addr *addr);
-#else
-static inline int
-string_to_ip6(const char *s, size_t len, struct in6_addr *addr)
-{
-	return -EINVAL;
-}
-#endif
 
 /**
  * ip6_is_unspecified_addr() - Check if IPv6 addr is not set i.e. is zero
@@ -209,10 +202,7 @@ string_to_ip6(const char *s, size_t len, struct in6_addr *addr)
  * @addr:	IPv6 addr
  * Return:  0 if addr is not set, -1 if is set
  */
-static inline int ip6_is_unspecified_addr(struct in6_addr *addr)
-{
-	return -1;
-}
+int ip6_is_unspecified_addr(struct in6_addr *addr);
 
 /**
  * ip6_is_our_addr() - Check if IPv6 addr belongs to our host addr
@@ -224,10 +214,7 @@ static inline int ip6_is_unspecified_addr(struct in6_addr *addr)
  * @addr:	addr to check
  * Return: 0 if addr is our, -1 otherwise
  */
-static inline int ip6_is_our_addr(struct in6_addr *addr)
-{
-	return -1;
-}
+int ip6_is_our_addr(struct in6_addr *addr);
 
 /**
  * ip6_addr_in_subnet() - Check if two IPv6 addresses are in the same subnet
@@ -237,12 +224,8 @@ static inline int ip6_is_our_addr(struct in6_addr *addr)
  * @prefix_length:	network mask length
  * Return: 0 if two addresses in the same subnet, -1 otherwise
  */
-static inline int
-ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
-		   u32 prefix_length)
-{
-	return -1;
-}
+int ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
+		       u32 prefix_length);
 
 /**
  * ip6_make_lladd() - rMake up IPv6 Link Local address
@@ -250,10 +233,7 @@ ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
  * @lladdr:	formed IPv6 Link Local address
  * @enetaddr:	MAC addr of a device
  */
-static inline void
-ip6_make_lladdr(struct in6_addr *lladdr, unsigned char const enetaddr[6])
-{
-}
+void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6]);
 
 /**
  * ip6_make_snma() - aMake up Solicited Node Multicast Address from IPv6 addr
@@ -261,10 +241,7 @@ ip6_make_lladdr(struct in6_addr *lladdr, unsigned char const enetaddr[6])
  * @mcast_addr:	formed SNMA addr
  * @ip6_addr:	base IPv6 addr
  */
-static inline void
-ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
-{
-}
+void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr);
 
 /**
  * ip6_make_mult_ethdstaddr() - Make up IPv6 multicast addr
@@ -272,11 +249,8 @@ ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
  * @enetaddr:	MAC addr of a device
  * @mcast_addr:	formed IPv6 multicast addr
  */
-static inline void
-ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
-			 struct in6_addr *mcast_addr)
-{
-}
+void ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
+			      struct in6_addr *mcast_addr);
 
 /**
  * csum_partial() - Compute an internet checksum
@@ -286,11 +260,7 @@ ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
  * @sum:	initial sum to be added in
  * Return: internet checksum of the buffer
  */
-static inline unsigned int
-csum_partial(const unsigned char *buff, int len, unsigned int sum)
-{
-	return 0;
-}
+unsigned int csum_partial(const unsigned char *buff, int len, unsigned int sum);
 
 /**
  * csum_ipv6_magic() - Compute checksum of IPv6 "psuedo-header" per RFC2460 section 8.1
@@ -302,13 +272,9 @@ csum_partial(const unsigned char *buff, int len, unsigned int sum)
  * @csum:	upper layer checksum
  * Return: computed checksum
  */
-static inline unsigned short
-csum_ipv6_magic(struct in6_addr *saddr,
-		struct in6_addr *daddr, u16 len,
-		unsigned short proto, unsigned int csum)
-{
-	return 0;
-}
+unsigned short int csum_ipv6_magic(struct in6_addr *saddr,
+				   struct in6_addr *daddr, u16 len,
+				   unsigned short proto, unsigned int csum);
 
 /**
  * ip6_add_hdr() - Make up IPv6 header
@@ -321,12 +287,8 @@ csum_ipv6_magic(struct in6_addr *saddr,
  * @payload_len: payload length
  * Return: IPv6 header length
  */
-static inline unsigned int
-ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
-	    int nextheader, int hoplimit, int payload_len)
-{
-	return 0;
-}
+int ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
+		int nextheader, int hoplimit, int payload_len);
 
 /**
  * net_send_udp_packet6() - Make up UDP packet and send it
@@ -338,12 +300,8 @@ ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
  * @len:	UDP packet length
  * Return: 0 if send successfully, -1 otherwise
  */
-static inline int
-net_send_udp_packet6(uchar *ether, struct in6_addr *dest,
-		     int dport, int sport, int len)
-{
-	return -1;
-}
+int net_send_udp_packet6(uchar *ether, struct in6_addr *dest, int dport,
+			 int sport, int len);
 
 /**
  * net_ip6_handler() - Handle IPv6 packet
@@ -353,12 +311,7 @@ net_send_udp_packet6(uchar *ether, struct in6_addr *dest,
  * @len:	incoming packet len
  * Return: 0 if handle packet successfully, -EINVAL in case of invalid protocol
  */
-static inline int
-net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6,
-		int len)
-{
-	return -EINVAL;
-}
+int net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len);
 
 /**
  * net_copy_ip6() - Copy IPv6 addr
@@ -368,6 +321,86 @@ net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6,
  */
 static inline void net_copy_ip6(void *to, const void *from)
 {
+	memcpy((void *)to, from, sizeof(struct in6_addr));
 }
+#else
+static inline int
+string_to_ip6(const char *s, size_t len, struct in6_addr *addr)
+{
+	return -EINVAL;
+}
+
+static inline int ip6_is_unspecified_addr(struct in6_addr *addr)
+{
+	return -1;
+}
+
+static inline int ip6_is_our_addr(struct in6_addr *addr)
+{
+	return -1;
+}
+
+static inline int
+ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
+		   u32 prefix_length)
+{
+	return -1;
+}
+
+static inline void
+ip6_make_lladdr(struct in6_addr *lladdr, unsigned char const enetaddr[6])
+{
+}
+
+static inline void
+ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
+{
+}
+
+static inline void
+ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
+			 struct in6_addr *mcast_addr)
+{
+}
+
+static inline unsigned int
+csum_partial(const unsigned char *buff, int len, unsigned int sum)
+{
+	return 0;
+}
+
+static inline unsigned short
+csum_ipv6_magic(struct in6_addr *saddr,
+		struct in6_addr *daddr, u16 len,
+		unsigned short proto, unsigned int csum)
+{
+	return 0;
+}
+
+static inline unsigned int
+ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
+	    int nextheader, int hoplimit, int payload_len)
+{
+	return 0;
+}
+
+static inline int
+net_send_udp_packet6(uchar *ether, struct in6_addr *dest,
+		     int dport, int sport, int len)
+{
+	return -1;
+}
+
+static inline int
+net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6,
+		int len)
+{
+	return -EINVAL;
+}
+
+static inline void net_copy_ip6(void *to, const void *from)
+{
+}
+#endif
 
 #endif /* __NET6_H__ */
diff --git a/net/net6.c b/net/net6.c
index cfbd4f5827d..e604227fe56 100644
--- a/net/net6.c
+++ b/net/net6.c
@@ -14,6 +14,7 @@
 #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;
@@ -77,3 +78,364 @@ static int on_serverip6(const char *name, const char *value, enum env_op op,
 }
 
 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_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;
+}