On Tue, Sep 6, 2022 at 6:10 PM Viacheslav Mitrofanov v.v.mitrofanov@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@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@gmail.com