[PATCH 08/17] net: ipv6: Add implementation of main IPv6 functions
Ramon Fried
rfried.dev at gmail.com
Mon Sep 12 09:17:37 CEST 2022
On Tue, Sep 6, 2022 at 6:10 PM Viacheslav Mitrofanov
<v.v.mitrofanov at yadro.com> wrote:
>
> 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.mitrofanov at yadro.com>
> ---
> include/net6.h | 161 +++++++++++++---------
> net/net6.c | 362 +++++++++++++++++++++++++++++++++++++++++++++++++
> 2 files changed, 459 insertions(+), 64 deletions(-)
>
> diff --git a/include/net6.h b/include/net6.h
> index d36fac26e1..cf52d023db 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
>
> /**
> * 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)
> * @param 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);
>
> /**
> * Check if IPv6 addr belongs to our host addr
> @@ -224,10 +214,7 @@ static inline int ip6_is_unspecified_addr(struct in6_addr *addr)
> * @param 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);
>
> /**
> * 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)
> * @param 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);
>
> /**
> * Make up IPv6 Link Local address
> @@ -250,10 +233,7 @@ ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
> * @param lladdr formed IPv6 Link Local address
> * @param 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]);
>
> /**
> * Make up Solicited Node Multicast Address from IPv6 addr
> @@ -261,10 +241,7 @@ ip6_make_lladdr(struct in6_addr *lladdr, unsigned char const enetaddr[6])
> * @param mcast_addr formed SNMA addr
> * @param 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);
>
> /**
> * Make up IPv6 multicast addr
> @@ -272,11 +249,8 @@ ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
> * @param enetaddr MAC addr of a device
> * @param 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);
>
> /**
> * Compute an internet checksum
> @@ -286,11 +260,7 @@ ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],
> * @param 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);
>
> /**
> * 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)
> * @param 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);
>
> /**
> * Make up IPv6 header
> @@ -321,12 +287,8 @@ csum_ipv6_magic(struct in6_addr *saddr,
> * @param 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);
>
> /**
> * Make up UDP packet and send it
> @@ -338,12 +300,8 @@ ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
> * @param 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);
>
> /**
> * Handle IPv6 packet
> @@ -353,12 +311,7 @@ net_send_udp_packet6(uchar *ether, struct in6_addr *dest,
> * @param 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);
>
> /**
> * 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 cfbd4f5827..e604227fe5 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;
> +}
> --
> 2.25.1
>
Reviewed-by: Ramon Fried <rfried.dev at gmail.com>
More information about the U-Boot
mailing list