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 --- 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 bae0461b11..972eb42034 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 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; +}