[04/17] net: ipv6: Add Neighbor Discovery Protocol (NDP)

Implement basic of NDP. It doesn't include such things as Router
Solicitation, Router Advertisement and Redirect. It just has Neighbor
Solicitation and Neighbor Advertisement. Only these two features are used
in u-boot IPv6. Implementation of some NDP functions uses API that was
exposed in "net: ipv6: Add IPv6 basic primitives".

Also this patch inlcudes update in Makefile to build NDP.

Signed-off-by: Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
---
 include/ndisc.h | 102 +++++++++++++++++
 net/Makefile    |   1 +
 net/ndisc.c     | 289 ++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 392 insertions(+)
 create mode 100644 include/ndisc.h
 create mode 100644 net/ndisc.c

On Tue, Sep 6, 2022 at 6:10 PM Viacheslav Mitrofanov
<v.v.mitrofanov@yadro.com> wrote:
>
> Implement basic of NDP. It doesn't include such things as Router
> Solicitation, Router Advertisement and Redirect. It just has Neighbor
> Solicitation and Neighbor Advertisement. Only these two features are used
> in u-boot IPv6. Implementation of some NDP functions uses API that was
> exposed in "net: ipv6: Add IPv6 basic primitives".
>
> Also this patch inlcudes update in Makefile to build NDP.
>
> Signed-off-by: Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
> ---
>  include/ndisc.h | 102 +++++++++++++++++
>  net/Makefile    |   1 +
>  net/ndisc.c     | 289 ++++++++++++++++++++++++++++++++++++++++++++++++
>  3 files changed, 392 insertions(+)
>  create mode 100644 include/ndisc.h
>  create mode 100644 net/ndisc.c
>
> diff --git a/include/ndisc.h b/include/ndisc.h
> new file mode 100644
> index 0000000000..26e6fcc85d
> --- /dev/null
> +++ b/include/ndisc.h
> @@ -0,0 +1,102 @@
> +/* SPDX-License-Identifier: GPL-2.0+ */
> +/*
> + * Copyright (C) 2013 Allied Telesis Labs NZ
> + * Chris Packham, <judge.packham@gmail.com>
> + *
> + * Copyright (C) 2022 YADRO
> + * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
> + */
> +
> +#ifndef __NDISC_H__
> +#define __NDISC_H__
> +
> +#include <ndisc.h>
> +
> +/* struct nd_msg - ICMPv6 Neighbour Discovery message format */
> +struct nd_msg {
> +       struct icmp6hdr icmph;
> +       struct in6_addr target;
> +       __u8            opt[0];
> +};
> +
> +/* struct echo_msg - ICMPv6 echo request/reply message format */
> +struct echo_msg {
> +       struct icmp6hdr icmph;
> +       __u16           id;
> +       __u16           sequence;
> +};
> +
> +/* Neigbour Discovery option types */
> +enum {
> +       __ND_OPT_PREFIX_INFO_END        = 0,
> +       ND_OPT_SOURCE_LL_ADDR           = 1,
> +       ND_OPT_TARGET_LL_ADDR           = 2,
> +       ND_OPT_PREFIX_INFO              = 3,
> +       ND_OPT_REDIRECT_HDR             = 4,
> +       ND_OPT_MTU                      = 5,
> +       __ND_OPT_MAX
> +};
> +
> +/* IPv6 destination address of packet waiting for ND */
> +extern struct in6_addr net_nd_sol_packet_ip6;
> +/* MAC destination address of packet waiting for ND */
> +extern uchar *net_nd_packet_mac;
> +/* pointer to packet waiting to be transmitted after ND is resolved */
> +extern uchar *net_nd_tx_packet;
> +/* size of packet waiting to be transmitted */
> +extern int net_nd_tx_packet_size;
> +/* the timer for ND resolution */
> +extern ulong net_nd_timer_start;
> +/* the number of requests we have sent so far */
> +extern int net_nd_try;
> +
> +#ifdef CONFIG_IPV6
> +/**
> + * Make initial steps for ND state machine. Usually move variables into
> + * initial state.
> + */
> +void ndisc_init(void);
> +
> +/**
> + * Handle ND packet
> + *
> + * @param et pointer to incoming packet
> + * @param ip6 pointer to IPv6 header
> + * @param len incoming packet length
> + * @return 0 if handle successfully, -1 if unsupported/unknown ND packet type
> + */
> +int ndisc_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len);
> +
> +/**
> + * Send ND request
> + */
> +void ndisc_request(void);
> +
> +/**
> + * Check ND response timeout
> + *
> + * @return 0 if no timeout, -1 otherwise
> + */
> +int ndisc_timeout_check(void);
> +#else
> +static inline void ndisc_init(void)
> +{
> +}
> +
> +static inline int
> +ndisc_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
> +{
> +       return -1;
> +}
> +
> +static inline void ndisc_request(void)
> +{
> +}
> +
> +static inline int ndisc_timeout_check(void)
> +{
> +       return 0;
> +}
> +#endif
> +
> +#endif /* __NDISC_H__ */
> diff --git a/net/Makefile b/net/Makefile
> index 4ea2a14f27..766dd04135 100644
> --- a/net/Makefile
> +++ b/net/Makefile
> @@ -20,6 +20,7 @@ obj-$(CONFIG_DM_MDIO)  += mdio-uclass.o
>  obj-$(CONFIG_DM_MDIO_MUX) += mdio-mux-uclass.o
>  obj-$(CONFIG_NET)      += eth_common.o
>  obj-$(CONFIG_CMD_LINK_LOCAL) += link_local.o
> +obj-$(CONFIG_IPV6)     += ndisc.o
>  obj-$(CONFIG_NET)      += net.o
>  obj-$(CONFIG_IPV6)     += net6.o
>  obj-$(CONFIG_CMD_NFS)  += nfs.o
> diff --git a/net/ndisc.c b/net/ndisc.c
> new file mode 100644
> index 0000000000..8d05c7404b
> --- /dev/null
> +++ b/net/ndisc.c
> @@ -0,0 +1,289 @@
> +// SPDX-License-Identifier: GPL-2.0+
> +/*
> + * Copyright (C) 2013 Allied Telesis Labs NZ
> + * Chris Packham, <judge.packham@gmail.com>
> + *
> + * Copyright (C) 2022 YADRO
> + * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
> + */
> +
> +/* Neighbour Discovery for IPv6 */
> +
> +#include <common.h>
> +#include <net.h>
> +#include <net6.h>
> +#include <ndisc.h>
> +
> +/* IPv6 destination address of packet waiting for ND */
> +struct in6_addr net_nd_sol_packet_ip6 = ZERO_IPV6_ADDR;
> +/* IPv6 address we are expecting ND advert from */
> +static struct in6_addr net_nd_rep_packet_ip6 = ZERO_IPV6_ADDR;
> +/* MAC destination address of packet waiting for ND */
> +uchar *net_nd_packet_mac;
> +/* pointer to packet waiting to be transmitted after ND is resolved */
> +uchar *net_nd_tx_packet;
> +static uchar net_nd_packet_buf[PKTSIZE_ALIGN + PKTALIGN];
> +/* size of packet waiting to be transmitted */
> +int net_nd_tx_packet_size;
> +/* the timer for ND resolution */
> +ulong net_nd_timer_start;
> +/* the number of requests we have sent so far */
> +int net_nd_try;
> +
> +#define IP6_NDISC_OPT_SPACE(len) (((len) + 2 + 7) & ~7)
> +
> +/**
> + * Insert an option into a neighbor discovery packet.
> + *
> + * @param ndisc pointer to ND packet
> + * @param type option type to insert
> + * @param data option data to insert
> + * @param len data length
> + * @return the number of bytes inserted (which may be >= len)
> + */
> +static int
> +ndisc_insert_option(struct nd_msg *ndisc, int type, u8 *data, int len)
> +{
> +       int space = IP6_NDISC_OPT_SPACE(len);
> +
> +       ndisc->opt[0] = type;
> +       ndisc->opt[1] = space >> 3;
> +       memcpy(&ndisc->opt[2], data, len);
> +       len += 2;
> +
> +       /* fill the remainder with 0 */
> +       if (space - len > 0)
> +               memset(&ndisc->opt[len], '\0', space - len);
> +
> +       return space;
> +}
> +
> +/**
> + * Extract the Ethernet address from a neighbor discovery packet
> + *
> + * Note that the link layer address could be anything but the only networking
> + * media that u-boot supports is Ethernet so we assume we're extracting a 6
> + * byte Ethernet MAC address.
> + *
> + * @param ndisc pointer to ND packet
> + * @param enetaddr extracted MAC addr
> + */
> +static void ndisc_extract_enetaddr(struct nd_msg *ndisc, uchar enetaddr[6])
> +{
> +       memcpy(enetaddr, &ndisc->opt[2], 6);
> +}
> +
> +/**
> + * Check if the neighbor discovery packet has the specified option set
> + *
> + * @param ip6 pointer to IPv6 header
> + * @param type option type to check
> + * @return 1 if ND has that option, 0 therwise
> + */
> +static int ndisc_has_option(struct ip6_hdr *ip6, __u8 type)
> +{
> +       struct nd_msg *ndisc = (struct nd_msg *)(((uchar *)ip6) + IP6_HDR_SIZE);
> +
> +       if (ip6->payload_len <= sizeof(struct icmp6hdr))
> +               return 0;
> +
> +       return ndisc->opt[0] == type;
> +}
> +
> +static void ip6_send_ns(struct in6_addr *neigh_addr)
> +{
> +       struct in6_addr dst_adr;
> +       unsigned char enetaddr[6];
> +       struct nd_msg *msg;
> +       __u16 len;
> +       uchar *pkt;
> +       unsigned short csum;
> +       unsigned int pcsum;
> +
> +       debug("sending neighbor solicitation for %pI6c our address %pI6c\n",
> +             neigh_addr, &net_link_local_ip6);
> +
> +       /* calculate src, dest IPv6 addr and dest Eth addr */
> +       ip6_make_snma(&dst_adr, neigh_addr);
> +       ip6_make_mult_ethdstaddr(enetaddr, &dst_adr);
> +       len = sizeof(struct icmp6hdr) + IN6ADDRSZ +
> +           IP6_NDISC_OPT_SPACE(INETHADDRSZ);
> +
> +       pkt = (uchar *)net_tx_packet;
> +       pkt += net_set_ether(pkt, enetaddr, PROT_IP6);
> +       pkt += ip6_add_hdr(pkt, &net_link_local_ip6, &dst_adr, PROT_ICMPV6,
> +                          IPV6_NDISC_HOPLIMIT, len);
> +
> +       /* ICMPv6 - NS */
> +       msg = (struct nd_msg *)pkt;
> +       msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_SOLICITATION;
> +       msg->icmph.icmp6_code = 0;
> +       memset(&msg->icmph.icmp6_cksum, 0, sizeof(__be16));
> +       memset(&msg->icmph.icmp6_unused, 0, sizeof(__be32));
> +
> +       /* Set the target address and llsaddr option */
> +       net_copy_ip6(&msg->target, neigh_addr);
> +       ndisc_insert_option(msg, ND_OPT_SOURCE_LL_ADDR, net_ethaddr,
> +                           INETHADDRSZ);
> +
> +       /* checksum */
> +       pcsum = csum_partial((__u8 *)msg, len, 0);
> +       csum = csum_ipv6_magic(&net_link_local_ip6, &dst_adr,
> +                              len, PROT_ICMPV6, pcsum);
> +       msg->icmph.icmp6_cksum = csum;
> +       pkt += len;
> +
> +       /* send it! */
> +       net_send_packet(net_tx_packet, (pkt - net_tx_packet));
> +}
> +
> +static void
> +ip6_send_na(uchar *eth_dst_addr, struct in6_addr *neigh_addr,
> +           struct in6_addr *target)
> +{
> +       struct nd_msg *msg;
> +       __u16 len;
> +       uchar *pkt;
> +       unsigned short csum;
> +
> +       debug("sending neighbor advertisement for %pI6c to %pI6c (%pM)\n",
> +             target, neigh_addr, eth_dst_addr);
> +
> +       len = sizeof(struct icmp6hdr) + IN6ADDRSZ +
> +           IP6_NDISC_OPT_SPACE(INETHADDRSZ);
> +
> +       pkt = (uchar *)net_tx_packet;
> +       pkt += net_set_ether(pkt, eth_dst_addr, PROT_IP6);
> +       pkt += ip6_add_hdr(pkt, &net_link_local_ip6, neigh_addr,
> +                          PROT_ICMPV6, IPV6_NDISC_HOPLIMIT, len);
> +
> +       /* ICMPv6 - NA */
> +       msg = (struct nd_msg *)pkt;
> +       msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT;
> +       msg->icmph.icmp6_code = 0;
> +       memset(&msg->icmph.icmp6_cksum, 0, sizeof(__be16));
> +       memset(&msg->icmph.icmp6_unused, 0, sizeof(__be32));
> +       msg->icmph.icmp6_dataun.u_nd_advt.solicited = 1;
> +       msg->icmph.icmp6_dataun.u_nd_advt.override = 1;
> +       /* Set the target address and lltargetaddr option */
> +       net_copy_ip6(&msg->target, target);
> +       ndisc_insert_option(msg, ND_OPT_TARGET_LL_ADDR, net_ethaddr,
> +                           INETHADDRSZ);
> +
> +       /* checksum */
> +       csum = csum_ipv6_magic(&net_link_local_ip6,
> +                              neigh_addr, len, PROT_ICMPV6,
> +                              csum_partial((__u8 *)msg, len, 0));
> +       msg->icmph.icmp6_cksum = csum;
> +       pkt += len;
> +
> +       /* send it! */
> +       net_send_packet(net_tx_packet, (pkt - net_tx_packet));
> +}
> +
> +void ndisc_request(void)
> +{
> +       if (!ip6_addr_in_subnet(&net_ip6, &net_nd_sol_packet_ip6,
> +                               net_prefix_length)) {
> +               if (ip6_is_unspecified_addr(&net_gateway6)) {
> +                       puts("## Warning: gatewayip6 is needed but not set\n");
> +                       net_nd_rep_packet_ip6 = net_nd_sol_packet_ip6;
> +               } else {
> +                       net_nd_rep_packet_ip6 = net_gateway6;
> +               }
> +       } else {
> +               net_nd_rep_packet_ip6 = net_nd_sol_packet_ip6;
> +       }
> +
> +       ip6_send_ns(&net_nd_rep_packet_ip6);
> +}
> +
> +int ndisc_timeout_check(void)
> +{
> +       ulong t;
> +
> +       if (ip6_is_unspecified_addr(&net_nd_sol_packet_ip6))
> +               return 0;
> +
> +       t = get_timer(0);
> +
> +       /* check for NDISC timeout */
> +       if ((t - net_nd_timer_start) > NDISC_TIMEOUT) {
> +               net_nd_try++;
> +               if (net_nd_try >= NDISC_TIMEOUT_COUNT) {
> +                       puts("\nNeighbour discovery retry count exceeded; "
> +                            "starting again\n");
> +                       net_nd_try = 0;
> +                       net_set_state(NETLOOP_FAIL);
> +               } else {
> +                       net_nd_timer_start = t;
> +                       ndisc_request();
> +               }
> +       }
> +       return 1;
> +}
> +
> +void ndisc_init(void)
> +{
> +       net_nd_packet_mac = NULL;
> +       net_nd_tx_packet = NULL;
> +       net_nd_sol_packet_ip6 = net_null_addr_ip6;
> +       net_nd_rep_packet_ip6 = net_null_addr_ip6;
> +       net_nd_tx_packet_size = 0;
> +       net_nd_tx_packet = &net_nd_packet_buf[0] + (PKTALIGN - 1);
> +       net_nd_tx_packet -= (ulong)net_nd_tx_packet % PKTALIGN;
> +}
> +
> +int ndisc_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
> +{
> +       struct icmp6hdr *icmp =
> +           (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
> +       struct nd_msg *ndisc = (struct nd_msg *)icmp;
> +       uchar neigh_eth_addr[6];
> +
> +       switch (icmp->icmp6_type) {
> +       case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
> +               debug("received neighbor solicitation for %pI6c from %pI6c\n",
> +                     &ndisc->target, &ip6->saddr);
> +               if (ip6_is_our_addr(&ndisc->target) &&
> +                   ndisc_has_option(ip6, ND_OPT_SOURCE_LL_ADDR)) {
> +                       ndisc_extract_enetaddr(ndisc, neigh_eth_addr);
> +                       ip6_send_na(neigh_eth_addr, &ip6->saddr,
> +                                   &ndisc->target);
> +               }
> +               break;
> +
> +       case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
> +               /* are we waiting for a reply ? */
> +               if (ip6_is_unspecified_addr(&net_nd_sol_packet_ip6))
> +                       break;
> +
> +               if ((memcmp(&ndisc->target, &net_nd_rep_packet_ip6,
> +                           sizeof(struct in6_addr)) == 0) &&
> +                   ndisc_has_option(ip6, ND_OPT_TARGET_LL_ADDR)) {
> +                       ndisc_extract_enetaddr(ndisc, neigh_eth_addr);
> +
> +                       /* save address for later use */
> +                       if (!net_nd_packet_mac)
> +                               memcpy(net_nd_packet_mac, neigh_eth_addr, 7);
> +
> +                       /* modify header, and transmit it */
> +                       memcpy(((struct ethernet_hdr *)net_nd_tx_packet)->et_dest,
> +                              neigh_eth_addr, 6);
> +
> +                       net_send_packet(net_nd_tx_packet,
> +                                       net_nd_tx_packet_size);
> +
> +                       /* no ND request pending now */
> +                       net_nd_sol_packet_ip6 = net_null_addr_ip6;
> +                       net_nd_tx_packet_size = 0;
> +                       net_nd_packet_mac = NULL;
> +               }
> +               break;
> +       default:
> +               debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
> +               return -1;
> +       }
> +
> +       return 0;
> +}
> --
> 2.25.1
>
Reviewed-by: Ramon Fried <rfried.dev@gmail.com>