net/net6.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 /* Simple IPv6 network layer implementation */
  11
  12 #include <common.h>
  13 #include <env_internal.h>
  14 #include <malloc.h>
  15 #include <net.h>
  16 #include <net6.h>
  17 #include <ndisc.h>
  18
  19 /* NULL IPv6 address */
  20 struct in6_addr const net_null_addr_ip6 = ZERO_IPV6_ADDR;
  21 /* Our gateway's IPv6 address */
  22 struct in6_addr net_gateway6 = ZERO_IPV6_ADDR;
  23 /* Our IPv6 addr (0 = unknown) */
  24 struct in6_addr net_ip6 = ZERO_IPV6_ADDR;
  25 /* Our link local IPv6 addr (0 = unknown) */
  26 struct in6_addr net_link_local_ip6 = ZERO_IPV6_ADDR;
  27 /* set server IPv6 addr (0 = unknown) */
  28 struct in6_addr net_server_ip6 = ZERO_IPV6_ADDR;
  29 /* The prefix length of our network */
  30 u32 net_prefix_length;
  31
  32 bool use_ip6;
  33
  34 static int on_ip6addr(const char *name, const char *value, enum env_op op,
  35                       int flags)
  36 {
  37         char *mask;
  38         size_t len;
  39
  40         if (flags & H_PROGRAMMATIC)
  41                 return 0;
  42
  43         if (op == env_op_delete) {
  44                 net_prefix_length = 0;
  45                 net_copy_ip6(&net_ip6, &net_null_addr_ip6);
  46                 return 0;
  47         }
  48
  49         mask = strchr(value, '/');
  50         len = strlen(value);
  51
  52         if (mask)
  53                 net_prefix_length = simple_strtoul(value + len, NULL, 10);
  54
  55         return string_to_ip6(value, len, &net_ip6);
  56 }
  57
  58 U_BOOT_ENV_CALLBACK(ip6addr, on_ip6addr);
  59
  60 static int on_gatewayip6(const char *name, const char *value, enum env_op op,
  61                          int flags)
  62 {
  63         if (flags & H_PROGRAMMATIC)
  64                 return 0;
  65
  66         return string_to_ip6(value, strlen(value), &net_gateway6);
  67 }
  68
  69 U_BOOT_ENV_CALLBACK(gatewayip6, on_gatewayip6);
  70
  71 static int on_serverip6(const char *name, const char *value, enum env_op op,
  72                         int flags)
  73 {
  74         if (flags & H_PROGRAMMATIC)
  75                 return 0;
  76
  77         return string_to_ip6(value, strlen(value), &net_server_ip6);
  78 }
  79
  80 U_BOOT_ENV_CALLBACK(serverip6, on_serverip6);
  81
  82 int ip6_is_unspecified_addr(struct in6_addr *addr)
  83 {
  84         return !(addr->s6_addr32[0] | addr->s6_addr32[1] |
  85                 addr->s6_addr32[2] | addr->s6_addr32[3]);
  86 }
  87
  88 int ip6_is_our_addr(struct in6_addr *addr)
  89 {
  90         return !memcmp(addr, &net_link_local_ip6, sizeof(struct in6_addr)) ||
  91                !memcmp(addr, &net_ip6, sizeof(struct in6_addr));
  92 }
  93
  94 void ip6_make_eui(unsigned char eui[8], unsigned char const enetaddr[6])
  95 {
  96         memcpy(eui, enetaddr, 3);
  97         memcpy(&eui[5], &enetaddr[3], 3);
  98         eui[3] = 0xff;
  99         eui[4] = 0xfe;
 100         eui[0] ^= 2;            /* "u" bit set to indicate global scope */
 101 }
 102
 103 void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6])
 104 {
 105         unsigned char eui[8];
 106
 107         memset(lladr, 0, sizeof(struct in6_addr));
 108         lladr->s6_addr16[0] = htons(IPV6_LINK_LOCAL_PREFIX);
 109         ip6_make_eui(eui, enetaddr);
 110         memcpy(&lladr->s6_addr[8], eui, 8);
 111 }
 112
 113 void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
 114 {
 115         memset(mcast_addr, 0, sizeof(struct in6_addr));
 116         mcast_addr->s6_addr[0] = 0xff;
 117         mcast_addr->s6_addr[1] = IPV6_ADDRSCOPE_LINK;
 118         mcast_addr->s6_addr[11] = 0x01;
 119         mcast_addr->s6_addr[12] = 0xff;
 120         mcast_addr->s6_addr[13] = ip6_addr->s6_addr[13];
 121         mcast_addr->s6_addr[14] = ip6_addr->s6_addr[14];
 122         mcast_addr->s6_addr[15] = ip6_addr->s6_addr[15];
 123 }
 124
 125 void
 126 ip6_make_mult_ethdstaddr(unsigned char enetaddr[6], struct in6_addr *mcast_addr)
 127 {
 128         enetaddr[0] = 0x33;
 129         enetaddr[1] = 0x33;
 130         memcpy(&enetaddr[2], &mcast_addr->s6_addr[12], 4);
 131 }
 132
 133 int
 134 ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
 135                    u32 plen)
 136 {
 137         __be32 *addr_dwords;
 138         __be32 *neigh_dwords;
 139
 140         addr_dwords = our_addr->s6_addr32;
 141         neigh_dwords = neigh_addr->s6_addr32;
 142
 143         while (plen > 32) {
 144                 if (*addr_dwords++ != *neigh_dwords++)
 145                         return 0;
 146
 147                 plen -= 32;
 148         }
 149
 150         /* Check any remaining bits */
 151         if (plen > 0) {
 152                 if ((*addr_dwords >> (32 - plen)) !=
 153                     (*neigh_dwords >> (32 - plen))) {
 154                         return 0;
 155                 }
 156         }
 157
 158         return 1;
 159 }
 160
 161 static inline unsigned int csum_fold(unsigned int sum)
 162 {
 163         sum = (sum & 0xffff) + (sum >> 16);
 164         sum = (sum & 0xffff) + (sum >> 16);
 165
 166         /* Opaque moment. If reverse it to zero it will not be checked on
 167          * receiver's side. It leads to bad negibour advertisement.
 168          */
 169         if (sum == 0xffff)
 170                 return sum;
 171
 172         return ~sum;
 173 }
 174
 175 static inline unsigned short from32to16(unsigned int x)
 176 {
 177         /* add up 16-bit and 16-bit for 16+c bit */
 178         x = (x & 0xffff) + (x >> 16);
 179         /* add up carry.. */
 180         x = (x & 0xffff) + (x >> 16);
 181         return x;
 182 }
 183
 184 static u32 csum_do_csum(const u8 *buff, int len)
 185 {
 186         int odd;
 187         unsigned int result = 0;
 188
 189         if (len <= 0)
 190                 goto out;
 191         odd = 1 & (unsigned long)buff;
 192         if (odd) {
 193 #ifdef __LITTLE_ENDIAN
 194                 result += (*buff << 8);
 195 #else
 196                 result = *buff;
 197 #endif
 198                 len--;
 199                 buff++;
 200         }
 201         if (len >= 2) {
 202                 if (2 & (unsigned long)buff) {
 203                         result += *(unsigned short *)buff;
 204                         len -= 2;
 205                         buff += 2;
 206                 }
 207                 if (len >= 4) {
 208                         const unsigned char *end = buff + ((u32)len & ~3);
 209                         unsigned int carry = 0;
 210
 211                         do {
 212                                 unsigned int w = *(unsigned int *)buff;
 213
 214                                 buff += 4;
 215                                 result += carry;
 216                                 result += w;
 217                                 carry = (w > result);
 218                         } while (buff < end);
 219                         result += carry;
 220                         result = (result & 0xffff) + (result >> 16);
 221                 }
 222                 if (len & 2) {
 223                         result += *(unsigned short *)buff;
 224                         buff += 2;
 225                 }
 226         }
 227         if (len & 1)
 228 #ifdef __LITTLE_ENDIAN
 229                 result += *buff;
 230 #else
 231                 result += (*buff << 8);
 232 #endif
 233         result = from32to16(result);
 234         if (odd)
 235                 result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
 236 out:
 237         return result;
 238 }
 239
 240 unsigned int csum_partial(const unsigned char *buff, int len, unsigned int sum)
 241 {
 242         unsigned int result = csum_do_csum(buff, len);
 243
 244         /* add in old sum, and carry.. */
 245         result += sum;
 246         /* 16+c bits -> 16 bits */
 247         result = (result & 0xffff) + (result >> 16);
 248         return result;
 249 }
 250
 251 unsigned short int
 252 csum_ipv6_magic(struct in6_addr *saddr, struct in6_addr *daddr, u16 len,
 253                 unsigned short proto, unsigned int csum)
 254 {
 255         int carry;
 256         u32 ulen;
 257         u32 uproto;
 258         u32 sum = csum;
 259
 260         sum += saddr->s6_addr32[0];
 261         carry = (sum < saddr->s6_addr32[0]);
 262         sum += carry;
 263
 264         sum += saddr->s6_addr32[1];
 265         carry = (sum < saddr->s6_addr32[1]);
 266         sum += carry;
 267
 268         sum += saddr->s6_addr32[2];
 269         carry = (sum < saddr->s6_addr32[2]);
 270         sum += carry;
 271
 272         sum += saddr->s6_addr32[3];
 273         carry = (sum < saddr->s6_addr32[3]);
 274         sum += carry;
 275
 276         sum += daddr->s6_addr32[0];
 277         carry = (sum < daddr->s6_addr32[0]);
 278         sum += carry;
 279
 280         sum += daddr->s6_addr32[1];
 281         carry = (sum < daddr->s6_addr32[1]);
 282         sum += carry;
 283
 284         sum += daddr->s6_addr32[2];
 285         carry = (sum < daddr->s6_addr32[2]);
 286         sum += carry;
 287
 288         sum += daddr->s6_addr32[3];
 289         carry = (sum < daddr->s6_addr32[3]);
 290         sum += carry;
 291
 292         ulen = htonl((u32)len);
 293         sum += ulen;
 294         carry = (sum < ulen);
 295         sum += carry;
 296
 297         uproto = htonl(proto);
 298         sum += uproto;
 299         carry = (sum < uproto);
 300         sum += carry;
 301
 302         return csum_fold(sum);
 303 }
 304
 305 int ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
 306                 int nextheader, int hoplimit, int payload_len)
 307 {
 308         struct ip6_hdr *ip6 = (struct ip6_hdr *)xip;
 309
 310         ip6->version = 6;
 311         ip6->priority = 0;
 312         ip6->flow_lbl[0] = 0;
 313         ip6->flow_lbl[1] = 0;
 314         ip6->flow_lbl[2] = 0;
 315         ip6->payload_len = htons(payload_len);
 316         ip6->nexthdr = nextheader;
 317         ip6->hop_limit = hoplimit;
 318         net_copy_ip6(&ip6->saddr, src);
 319         net_copy_ip6(&ip6->daddr, dest);
 320
 321         return sizeof(struct ip6_hdr);
 322 }
 323
 324 int net_send_udp_packet6(uchar *ether, struct in6_addr *dest, int dport,
 325                          int sport, int len)
 326 {
 327         uchar *pkt;
 328         struct udp_hdr *udp;
 329         u16 csum_p;
 330
 331         udp = (struct udp_hdr *)((uchar *)net_tx_packet + net_eth_hdr_size() +
 332                         IP6_HDR_SIZE);
 333
 334         udp->udp_dst = htons(dport);
 335         udp->udp_src = htons(sport);
 336         udp->udp_len = htons(len + UDP_HDR_SIZE);
 337
 338         /* checksum */
 339         udp->udp_xsum = 0;
 340         csum_p = csum_partial((u8 *)udp, len + UDP_HDR_SIZE, 0);
 341         udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
 342                                         IPPROTO_UDP, csum_p);
 343
 344         /* if MAC address was not discovered yet, save the packet and do
 345          * neighbour discovery
 346          */
 347         if (!memcmp(ether, net_null_ethaddr, 6)) {
 348                 net_copy_ip6(&net_nd_sol_packet_ip6, dest);
 349                 net_nd_packet_mac = ether;
 350
 351                 pkt = net_nd_tx_packet;
 352                 pkt += net_set_ether(pkt, net_nd_packet_mac, PROT_IP6);
 353                 pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
 354                                 len + UDP_HDR_SIZE);
 355                 memcpy(pkt, (uchar *)udp, len + UDP_HDR_SIZE);
 356
 357                 /* size of the waiting packet */
 358                 net_nd_tx_packet_size = (pkt - net_nd_tx_packet) +
 359                         UDP_HDR_SIZE + len;
 360
 361                 /* and do the neighbor solicitation */
 362                 net_nd_try = 1;
 363                 net_nd_timer_start = get_timer(0);
 364                 ndisc_request();
 365                 return 1;       /* waiting */
 366         }
 367
 368         pkt = (uchar *)net_tx_packet;
 369         pkt += net_set_ether(pkt, ether, PROT_IP6);
 370         pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
 371                         len + UDP_HDR_SIZE);
 372         (void)eth_send(net_tx_packet, pkt - net_tx_packet + UDP_HDR_SIZE + len);
 373
 374         return 0;       /* transmitted */
 375 }
 376
 377 int net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
 378 {
 379         struct in_addr zero_ip = {.s_addr = 0 };
 380         struct icmp6hdr *icmp;
 381         struct udp_hdr *udp;
 382         u16 csum;
 383         u16 csum_p;
 384         u16 hlen;
 385
 386         if (len < IP6_HDR_SIZE)
 387                 return -EINVAL;
 388
 389         if (ip6->version != 6)
 390                 return -EINVAL;
 391
 392         switch (ip6->nexthdr) {
 393         case PROT_ICMPV6:
 394                 icmp = (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
 395                 csum = icmp->icmp6_cksum;
 396                 hlen = ntohs(ip6->payload_len);
 397                 icmp->icmp6_cksum = 0;
 398                 /* checksum */
 399                 csum_p = csum_partial((u8 *)icmp, hlen, 0);
 400                 icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
 401                                                     hlen, PROT_ICMPV6, csum_p);
 402
 403                 if (icmp->icmp6_cksum != csum)
 404                         return -EINVAL;
 405
 406                 switch (icmp->icmp6_type) {
 407                 case IPV6_ICMP_ECHO_REQUEST:
 408                 case IPV6_ICMP_ECHO_REPLY:
 409                         ping6_receive(et, ip6, len);
 410                         break;
 411                 case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
 412                 case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
 413                         ndisc_receive(et, ip6, len);
 414                         break;
 415                 default:
 416                         break;
 417                 }
 418                 break;
 419         case IPPROTO_UDP:
 420                 udp = (struct udp_hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
 421                 csum = udp->udp_xsum;
 422                 hlen = ntohs(ip6->payload_len);
 423                 udp->udp_xsum = 0;
 424                 /* checksum */
 425                 csum_p = csum_partial((u8 *)udp, hlen, 0);
 426                 udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
 427                                                 hlen, IPPROTO_UDP, csum_p);
 428
 429                 if (csum != udp->udp_xsum)
 430                         return -EINVAL;
 431
 432                 /* IP header OK. Pass the packet to the current handler. */
 433                 net_get_udp_handler()((uchar *)ip6 + IP6_HDR_SIZE +
 434                                         UDP_HDR_SIZE,
 435                                 ntohs(udp->udp_dst),
 436                                 zero_ip,
 437                                 ntohs(udp->udp_src),
 438                                 ntohs(udp->udp_len) - 8);
 439                 break;
 440         default:
 441                 return -EINVAL;
 442         }
 443
 444         return 0;
 445 }