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