net/ndisc.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (C) 2013 Allied Telesis Labs NZ
   4  * Chris Packham, <judge.packham@gmail.com>
   5  *
   6  * Copyright (C) 2022 YADRO
   7  * Viacheslav Mitrofanov <v.v.mitrofanov@yadro.com>
   8  */
   9
  10 /* Neighbour Discovery for IPv6 */
  11
  12 #include <common.h>
  13 #include <net.h>
  14 #include <net6.h>
  15 #include <ndisc.h>
  16
  17 /* IPv6 destination address of packet waiting for ND */
  18 struct in6_addr net_nd_sol_packet_ip6 = ZERO_IPV6_ADDR;
  19 /* IPv6 address we are expecting ND advert from */
  20 static struct in6_addr net_nd_rep_packet_ip6 = ZERO_IPV6_ADDR;
  21 /* MAC destination address of packet waiting for ND */
  22 uchar *net_nd_packet_mac;
  23 /* pointer to packet waiting to be transmitted after ND is resolved */
  24 uchar *net_nd_tx_packet;
  25 static uchar net_nd_packet_buf[PKTSIZE_ALIGN + PKTALIGN];
  26 /* size of packet waiting to be transmitted */
  27 int net_nd_tx_packet_size;
  28 /* the timer for ND resolution */
  29 ulong net_nd_timer_start;
  30 /* the number of requests we have sent so far */
  31 int net_nd_try;
  32
  33 #define IP6_NDISC_OPT_SPACE(len) (((len) + 2 + 7) & ~7)
  34
  35 /**
  36  * ndisc_insert_option() - Insert an option into a neighbor discovery packet
  37  *
  38  * @ndisc:      pointer to ND packet
  39  * @type:       option type to insert
  40  * @data:       option data to insert
  41  * @len:        data length
  42  * Return: the number of bytes inserted (which may be >= len)
  43  */
  44 static int
  45 ndisc_insert_option(struct nd_msg *ndisc, int type, u8 *data, int len)
  46 {
  47         int space = IP6_NDISC_OPT_SPACE(len);
  48
  49         ndisc->opt[0] = type;
  50         ndisc->opt[1] = space >> 3;
  51         memcpy(&ndisc->opt[2], data, len);
  52         len += 2;
  53
  54         /* fill the remainder with 0 */
  55         if (space - len > 0)
  56                 memset(&ndisc->opt[len], '\0', space - len);
  57
  58         return space;
  59 }
  60
  61 /**
  62  * ndisc_extract_enetaddr() - Extract the Ethernet address from a ND packet
  63  *
  64  * Note that the link layer address could be anything but the only networking
  65  * media that u-boot supports is Ethernet so we assume we're extracting a 6
  66  * byte Ethernet MAC address.
  67  *
  68  * @ndisc:      pointer to ND packet
  69  * @enetaddr:   extracted MAC addr
  70  */
  71 static void ndisc_extract_enetaddr(struct nd_msg *ndisc, uchar enetaddr[6])
  72 {
  73         memcpy(enetaddr, &ndisc->opt[2], 6);
  74 }
  75
  76 /**
  77  * ndisc_has_option() - Check if the ND packet has the specified option set
  78  *
  79  * @ip6:        pointer to IPv6 header
  80  * @type:       option type to check
  81  * Return: 1 if ND has that option, 0 therwise
  82  */
  83 static int ndisc_has_option(struct ip6_hdr *ip6, __u8 type)
  84 {
  85         struct nd_msg *ndisc = (struct nd_msg *)(((uchar *)ip6) + IP6_HDR_SIZE);
  86
  87         if (ip6->payload_len <= sizeof(struct icmp6hdr))
  88                 return 0;
  89
  90         return ndisc->opt[0] == type;
  91 }
  92
  93 static void ip6_send_ns(struct in6_addr *neigh_addr)
  94 {
  95         struct in6_addr dst_adr;
  96         unsigned char enetaddr[6];
  97         struct nd_msg *msg;
  98         __u16 len;
  99         uchar *pkt;
 100         unsigned short csum;
 101         unsigned int pcsum;
 102
 103         debug("sending neighbor solicitation for %pI6c our address %pI6c\n",
 104               neigh_addr, &net_link_local_ip6);
 105
 106         /* calculate src, dest IPv6 addr and dest Eth addr */
 107         ip6_make_snma(&dst_adr, neigh_addr);
 108         ip6_make_mult_ethdstaddr(enetaddr, &dst_adr);
 109         len = sizeof(struct icmp6hdr) + IN6ADDRSZ +
 110             IP6_NDISC_OPT_SPACE(INETHADDRSZ);
 111
 112         pkt = (uchar *)net_tx_packet;
 113         pkt += net_set_ether(pkt, enetaddr, PROT_IP6);
 114         pkt += ip6_add_hdr(pkt, &net_link_local_ip6, &dst_adr, PROT_ICMPV6,
 115                            IPV6_NDISC_HOPLIMIT, len);
 116
 117         /* ICMPv6 - NS */
 118         msg = (struct nd_msg *)pkt;
 119         msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_SOLICITATION;
 120         msg->icmph.icmp6_code = 0;
 121         memset(&msg->icmph.icmp6_cksum, 0, sizeof(__be16));
 122         memset(&msg->icmph.icmp6_unused, 0, sizeof(__be32));
 123
 124         /* Set the target address and llsaddr option */
 125         net_copy_ip6(&msg->target, neigh_addr);
 126         ndisc_insert_option(msg, ND_OPT_SOURCE_LL_ADDR, net_ethaddr,
 127                             INETHADDRSZ);
 128
 129         /* checksum */
 130         pcsum = csum_partial((__u8 *)msg, len, 0);
 131         csum = csum_ipv6_magic(&net_link_local_ip6, &dst_adr,
 132                                len, PROT_ICMPV6, pcsum);
 133         msg->icmph.icmp6_cksum = csum;
 134         pkt += len;
 135
 136         /* send it! */
 137         net_send_packet(net_tx_packet, (pkt - net_tx_packet));
 138 }
 139
 140 static void
 141 ip6_send_na(uchar *eth_dst_addr, struct in6_addr *neigh_addr,
 142             struct in6_addr *target)
 143 {
 144         struct nd_msg *msg;
 145         __u16 len;
 146         uchar *pkt;
 147         unsigned short csum;
 148
 149         debug("sending neighbor advertisement for %pI6c to %pI6c (%pM)\n",
 150               target, neigh_addr, eth_dst_addr);
 151
 152         len = sizeof(struct icmp6hdr) + IN6ADDRSZ +
 153             IP6_NDISC_OPT_SPACE(INETHADDRSZ);
 154
 155         pkt = (uchar *)net_tx_packet;
 156         pkt += net_set_ether(pkt, eth_dst_addr, PROT_IP6);
 157         pkt += ip6_add_hdr(pkt, &net_link_local_ip6, neigh_addr,
 158                            PROT_ICMPV6, IPV6_NDISC_HOPLIMIT, len);
 159
 160         /* ICMPv6 - NA */
 161         msg = (struct nd_msg *)pkt;
 162         msg->icmph.icmp6_type = IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT;
 163         msg->icmph.icmp6_code = 0;
 164         memset(&msg->icmph.icmp6_cksum, 0, sizeof(__be16));
 165         memset(&msg->icmph.icmp6_unused, 0, sizeof(__be32));
 166         msg->icmph.icmp6_dataun.u_nd_advt.solicited = 1;
 167         msg->icmph.icmp6_dataun.u_nd_advt.override = 1;
 168         /* Set the target address and lltargetaddr option */
 169         net_copy_ip6(&msg->target, target);
 170         ndisc_insert_option(msg, ND_OPT_TARGET_LL_ADDR, net_ethaddr,
 171                             INETHADDRSZ);
 172
 173         /* checksum */
 174         csum = csum_ipv6_magic(&net_link_local_ip6,
 175                                neigh_addr, len, PROT_ICMPV6,
 176                                csum_partial((__u8 *)msg, len, 0));
 177         msg->icmph.icmp6_cksum = csum;
 178         pkt += len;
 179
 180         /* send it! */
 181         net_send_packet(net_tx_packet, (pkt - net_tx_packet));
 182 }
 183
 184 void ndisc_request(void)
 185 {
 186         if (!ip6_addr_in_subnet(&net_ip6, &net_nd_sol_packet_ip6,
 187                                 net_prefix_length)) {
 188                 if (ip6_is_unspecified_addr(&net_gateway6)) {
 189                         puts("## Warning: gatewayip6 is needed but not set\n");
 190                         net_nd_rep_packet_ip6 = net_nd_sol_packet_ip6;
 191                 } else {
 192                         net_nd_rep_packet_ip6 = net_gateway6;
 193                 }
 194         } else {
 195                 net_nd_rep_packet_ip6 = net_nd_sol_packet_ip6;
 196         }
 197
 198         ip6_send_ns(&net_nd_rep_packet_ip6);
 199 }
 200
 201 int ndisc_timeout_check(void)
 202 {
 203         ulong t;
 204
 205         if (ip6_is_unspecified_addr(&net_nd_sol_packet_ip6))
 206                 return 0;
 207
 208         t = get_timer(0);
 209
 210         /* check for NDISC timeout */
 211         if ((t - net_nd_timer_start) > NDISC_TIMEOUT) {
 212                 net_nd_try++;
 213                 if (net_nd_try >= NDISC_TIMEOUT_COUNT) {
 214                         puts("\nNeighbour discovery retry count exceeded; "
 215                              "starting again\n");
 216                         net_nd_try = 0;
 217                         net_set_state(NETLOOP_FAIL);
 218                 } else {
 219                         net_nd_timer_start = t;
 220                         ndisc_request();
 221                 }
 222         }
 223         return 1;
 224 }
 225
 226 void ndisc_init(void)
 227 {
 228         net_nd_packet_mac = NULL;
 229         net_nd_tx_packet = NULL;
 230         net_nd_sol_packet_ip6 = net_null_addr_ip6;
 231         net_nd_rep_packet_ip6 = net_null_addr_ip6;
 232         net_nd_tx_packet_size = 0;
 233         net_nd_tx_packet = &net_nd_packet_buf[0] + (PKTALIGN - 1);
 234         net_nd_tx_packet -= (ulong)net_nd_tx_packet % PKTALIGN;
 235 }
 236
 237 int ndisc_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 238 {
 239         struct icmp6hdr *icmp =
 240             (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
 241         struct nd_msg *ndisc = (struct nd_msg *)icmp;
 242         uchar neigh_eth_addr[6];
 243
 244         switch (icmp->icmp6_type) {
 245         case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
 246                 debug("received neighbor solicitation for %pI6c from %pI6c\n",
 247                       &ndisc->target, &ip6->saddr);
 248                 if (ip6_is_our_addr(&ndisc->target) &&
 249                     ndisc_has_option(ip6, ND_OPT_SOURCE_LL_ADDR)) {
 250                         ndisc_extract_enetaddr(ndisc, neigh_eth_addr);
 251                         ip6_send_na(neigh_eth_addr, &ip6->saddr,
 252                                     &ndisc->target);
 253                 }
 254                 break;
 255
 256         case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
 257                 /* are we waiting for a reply ? */
 258                 if (ip6_is_unspecified_addr(&net_nd_sol_packet_ip6))
 259                         break;
 260
 261                 if ((memcmp(&ndisc->target, &net_nd_rep_packet_ip6,
 262                             sizeof(struct in6_addr)) == 0) &&
 263                     ndisc_has_option(ip6, ND_OPT_TARGET_LL_ADDR)) {
 264                         ndisc_extract_enetaddr(ndisc, neigh_eth_addr);
 265
 266                         /* save address for later use */
 267                         if (!net_nd_packet_mac)
 268                                 net_nd_packet_mac = neigh_eth_addr;
 269
 270                         /* modify header, and transmit it */
 271                         memcpy(((struct ethernet_hdr *)net_nd_tx_packet)->et_dest,
 272                                neigh_eth_addr, 6);
 273
 274                         net_send_packet(net_nd_tx_packet,
 275                                         net_nd_tx_packet_size);
 276
 277                         /* no ND request pending now */
 278                         net_nd_sol_packet_ip6 = net_null_addr_ip6;
 279                         net_nd_tx_packet_size = 0;
 280                         net_nd_packet_mac = NULL;
 281                 }
 282                 break;
 283         default:
 284                 debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
 285                 return -1;
 286         }
 287
 288         return 0;
 289 }