From patchwork Fri Dec 2 09:18:05 2022 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: "Vyacheslav V. Mitrofanov" X-Patchwork-Id: 1711317 X-Patchwork-Delegate: trini@ti.com Return-Path: X-Original-To: incoming@patchwork.ozlabs.org Delivered-To: patchwork-incoming@legolas.ozlabs.org Authentication-Results: legolas.ozlabs.org; spf=pass (sender SPF authorized) smtp.mailfrom=lists.denx.de (client-ip=85.214.62.61; helo=phobos.denx.de; envelope-from=u-boot-bounces@lists.denx.de; receiver=) Authentication-Results: legolas.ozlabs.org; dkim=pass (1024-bit key; unprotected) header.d=yadro.com header.i=@yadro.com header.a=rsa-sha256 header.s=mta-01 header.b=GyD2j9xW; dkim-atps=neutral Received: from phobos.denx.de (phobos.denx.de [85.214.62.61]) (using TLSv1.3 with cipher TLS_AES_256_GCM_SHA384 (256/256 bits) key-exchange X25519 server-signature ECDSA (P-384)) (No client certificate requested) by legolas.ozlabs.org (Postfix) with ESMTPS id 4NNnVh5WqLz23nB for ; Fri, 2 Dec 2022 20:21:00 +1100 (AEDT) Received: from h2850616.stratoserver.net (localhost [IPv6:::1]) by phobos.denx.de (Postfix) with ESMTP id 29C6D85409; Fri, 2 Dec 2022 10:19:34 +0100 (CET) Authentication-Results: phobos.denx.de; dmarc=pass (p=none dis=none) header.from=yadro.com Authentication-Results: phobos.denx.de; spf=pass smtp.mailfrom=u-boot-bounces@lists.denx.de Authentication-Results: phobos.denx.de; dkim=pass (1024-bit key; unprotected) header.d=yadro.com header.i=@yadro.com header.b="GyD2j9xW"; dkim-atps=neutral Received: by phobos.denx.de (Postfix, from userid 109) id 86980853D1; Fri, 2 Dec 2022 10:19:16 +0100 (CET) X-Spam-Checker-Version: SpamAssassin 3.4.2 (2018-09-13) on phobos.denx.de X-Spam-Level: X-Spam-Status: No, score=-2.1 required=5.0 tests=BAYES_00,DKIM_SIGNED, DKIM_VALID,DKIM_VALID_AU,DKIM_VALID_EF,SPF_HELO_NONE,SPF_PASS autolearn=unavailable autolearn_force=no version=3.4.2 Received: from mta-01.yadro.com (mta-02.yadro.com [89.207.88.252]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-GCM-SHA384 (256/256 bits)) (No client certificate requested) by phobos.denx.de (Postfix) with ESMTPS id 0B6D0853D0 for ; Fri, 2 Dec 2022 10:18:56 +0100 (CET) Authentication-Results: phobos.denx.de; dmarc=pass (p=none dis=none) header.from=yadro.com Authentication-Results: phobos.denx.de; spf=pass smtp.mailfrom=v.v.mitrofanov@yadro.com Received: from localhost (unknown [127.0.0.1]) by mta-01.yadro.com (Postfix) with ESMTP id A96044088A; Fri, 2 Dec 2022 09:18:55 +0000 (UTC) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/simple; d=yadro.com; h= content-type:content-type:content-transfer-encoding:mime-version :references:in-reply-to:x-mailer:message-id:date:date:subject :subject:from:from:received:received:received:received; s= mta-01; t=1669972733; x=1671787134; bh=FvdB5VHL9vLji72q9MOxN6x5S WJNTdX8S/C+VJx8hwI=; b=GyD2j9xWt2+TR2OI+byxK5ThDpb+6Fh07K1miSP+W PzA9M+sfXhR6WWUyGipDRqvfVa2VDlha6MBWXKvgwsq7ugsjHtVsjHLo5G/2Ycod IxisIi2s8wpvaV8woQRRtqugw92XnhEpHd9geMjZItw3qPt1cX0BktWLCikfrS5u gU= Received: from mta-01.yadro.com ([127.0.0.1]) by localhost (mta-01.yadro.com [127.0.0.1]) (amavisd-new, port 10024) with ESMTP id WtpnxbO_bbqZ; Fri, 2 Dec 2022 12:18:53 +0300 (MSK) Received: from T-EXCH-02.corp.yadro.com (T-EXCH-02.corp.yadro.com [172.17.10.102]) (using TLSv1.2 with cipher ECDHE-RSA-AES256-SHA384 (256/256 bits)) (No client certificate requested) by mta-01.yadro.com (Postfix) with ESMTPS id 703014014D; Fri, 2 Dec 2022 12:18:53 +0300 (MSK) Received: from T-Exch-05.corp.yadro.com (172.17.10.109) by T-EXCH-02.corp.yadro.com (172.17.10.102) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_CBC_SHA384_P384) id 15.1.669.32; Fri, 2 Dec 2022 12:18:53 +0300 Received: from v.yadro.com (10.178.114.10) by T-Exch-05.corp.yadro.com (172.17.10.109) with Microsoft SMTP Server (version=TLS1_2, cipher=TLS_ECDHE_RSA_WITH_AES_256_CBC_SHA384) id 15.2.1118.9; Fri, 2 Dec 2022 12:18:52 +0300 From: Viacheslav Mitrofanov To: CC: , , , , , , Subject: [PATCH v5 08/19] net: ipv6: Add implementation of main IPv6 functions Date: Fri, 2 Dec 2022 12:18:05 +0300 Message-ID: <20221202091818.2382748-9-v.v.mitrofanov@yadro.com> X-Mailer: git-send-email 2.25.1 In-Reply-To: <20221202091818.2382748-1-v.v.mitrofanov@yadro.com> References: <20221202091818.2382748-1-v.v.mitrofanov@yadro.com> MIME-Version: 1.0 X-Originating-IP: [10.178.114.10] X-ClientProxiedBy: T-EXCH-01.corp.yadro.com (172.17.10.101) To T-Exch-05.corp.yadro.com (172.17.10.109) X-BeenThere: u-boot@lists.denx.de X-Mailman-Version: 2.1.39 Precedence: list List-Id: U-Boot discussion List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , Errors-To: u-boot-bounces@lists.denx.de Sender: "U-Boot" X-Virus-Scanned: clamav-milter 0.103.6 at phobos.denx.de X-Virus-Status: Clean 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 Reviewed-by: Ramon Fried Reviewed-by: Simon Glass --- 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 #include #include +#include /* 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; +}