Viacheslav Mitrofanov | 33b5066 | 2022-12-02 12:17:58 +0300 | [diff] [blame] | 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 | |
Tom Rini | d678a59 | 2024-05-18 20:20:43 -0600 | [diff] [blame] | 12 | #include <common.h> |
Viacheslav Mitrofanov | 33b5066 | 2022-12-02 12:17:58 +0300 | [diff] [blame] | 13 | #include <env_internal.h> |
| 14 | #include <malloc.h> |
| 15 | #include <net.h> |
| 16 | #include <net6.h> |
Viacheslav Mitrofanov | 1feb697 | 2022-12-02 12:18:05 +0300 | [diff] [blame] | 17 | #include <ndisc.h> |
Viacheslav Mitrofanov | 33b5066 | 2022-12-02 12:17:58 +0300 | [diff] [blame] | 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; |
Viacheslav Mitrofanov | 43ab8ec | 2022-12-02 12:18:04 +0300 | [diff] [blame] | 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, '/'); |
Viacheslav Mitrofanov | 43ab8ec | 2022-12-02 12:18:04 +0300 | [diff] [blame] | 50 | |
Sean Edmond | 796e549 | 2023-01-06 14:22:55 -0800 | [diff] [blame] | 51 | if (mask) { |
| 52 | net_prefix_length = simple_strtoul(mask + 1, NULL, 10); |
| 53 | len = mask - value; |
| 54 | } else { |
| 55 | len = strlen(value); |
| 56 | } |
Viacheslav Mitrofanov | 43ab8ec | 2022-12-02 12:18:04 +0300 | [diff] [blame] | 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); |
Viacheslav Mitrofanov | 1feb697 | 2022-12-02 12:18:05 +0300 | [diff] [blame] | 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) { |
Viacheslav Mitrofanov | eeb0a2c | 2022-12-02 12:18:08 +0300 | [diff] [blame] | 410 | case IPV6_ICMP_ECHO_REQUEST: |
| 411 | case IPV6_ICMP_ECHO_REPLY: |
| 412 | ping6_receive(et, ip6, len); |
| 413 | break; |
Viacheslav Mitrofanov | 1feb697 | 2022-12-02 12:18:05 +0300 | [diff] [blame] | 414 | case IPV6_NDISC_NEIGHBOUR_SOLICITATION: |
| 415 | case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT: |
Ehsan Mohandesi | 6de98b6 | 2023-04-21 17:08:21 -0700 | [diff] [blame] | 416 | case IPV6_NDISC_ROUTER_ADVERTISEMENT: |
Viacheslav Mitrofanov | 1feb697 | 2022-12-02 12:18:05 +0300 | [diff] [blame] | 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 | } |