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.
Signed-off-by: Viacheslav Mitrofanov <v.v.mitrofa...@yadro.com>
---
include/net6.h | 36 ++++-
net/net6.c | 380 +++++++++++++++++++++++++++++++++++++++++++++++++
2 files changed, 415 insertions(+), 1 deletion(-)
diff --git a/include/net6.h b/include/net6.h
index 68fa38adbb..a7be2496d9 100644
--- a/include/net6.h
+++ b/include/net6.h
@@ -180,12 +180,45 @@ extern bool use_ip6;
#if IS_ENABLED(CONFIG_IPV6)
/* Convert a string to an ipv6 address */
int string_to_ip6(const char *s, struct in6_addr *addr);
+
+/* check that an IPv6 address is unspecified (zero) */
+int ip6_is_unspecified_addr(struct in6_addr *addr);
+/* check that an IPv6 address is ours */
+int ip6_is_our_addr(struct in6_addr *addr);
+/* check if neighbour is in the same subnet as us */
+int ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
+ u32 prefix_length);
+
+void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6]);
+void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr);
+void ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
+ struct in6_addr *mcast_addr);
+
+unsigned int csum_partial(const unsigned char *buff, int len, unsigned int
sum);
+unsigned short int csum_ipv6_magic(struct in6_addr *saddr,
+ struct in6_addr *daddr, u16 len,
+ unsigned short proto, unsigned int csum);
+
+int ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
+ int nextheader, int hoplimit, int payload_len);
+
+/* Transmit UDP packet using IPv6, performing neighbour discovery if needed */
+int net_send_udp_packet6(uchar *ether, struct in6_addr *dest,
+ int dport, int sport, int len);
+
+/* handler for incoming IPv6 echo packet */
+void net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6,
+ int len);
+
+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, struct in6_addr *addr)
{
return -1;
}
-#endif
static inline int ip6_is_unspecified_addr(struct in6_addr *addr)
{
@@ -257,5 +290,6 @@ net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr
*ip6,
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 a0410ea8ba..0799d411b2 100644
--- a/net/net6.c
+++ b/net/net6.c
@@ -16,6 +16,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;
@@ -98,3 +99,382 @@ 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]) == 0;
+}
+
+/**
+ * We have 2 addresses that we should respond to. A link
+ * local address and a global address. This returns true
+ * if the specified address matches either of these.
+ */
+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])
+{
+ uchar 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);
+}
+
+/*
+ * Given an IPv6 address generate an equivalent Solicited Node Multicast
+ * Address (SNMA) as described in RFC2461.
+ */
+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];
+}
+
+/*
+ * Given an IPv6 address generate the multicast MAC address that corresponds to
+ * it.
+ */
+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 for some reason.
+ */
+ 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;
+}
+
+/*
+ * Compute checksum of IPv6 "psuedo-header" per RFC2460 section 8.1
+ */
+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;
+
+ 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;
+ udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
+ IPPROTO_UDP,
+ csum_partial((u8 *)udp,
+ len + UDP_HDR_SIZE,
+ 0));
+
+ /* if MAC address was not discovered yet, save the packet and do
+ * neighbour discovery
+ */
+ if (memcmp(ether, net_null_ethaddr, 6) == 0) {
+ 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 */
+}
+
+void 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 hlen;
+
+ if (len < IP6_HDR_SIZE)
+ return;
+
+ if (ip6->version != 6)
+ return;
+
+ switch (ip6->nexthdr) {
+ case IPPROTO_ICMPV6:
+ icmp = (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
+ csum = icmp->icmp6_cksum;
+ hlen = ntohs(ip6->payload_len);
+ icmp->icmp6_cksum = 0;
+ /* checksum */
+ icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
+ hlen, IPPROTO_ICMPV6,
+ csum_partial((u8 *)icmp,
+ hlen, 0));
+
+ if (icmp->icmp6_cksum != csum)
+ return;
+
+ 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 */
+ udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
+ hlen, IPPROTO_UDP,
+ csum_partial((u8 *)udp,
+ hlen, 0));
+
+ if (csum != udp->udp_xsum)
+ return;
+
+ /* 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:
+ break;
+ }
+}
--
2.25.1
