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 */ 20struct in6_addr const net_null_addr_ip6 = ZERO_IPV6_ADDR; 21/* Our gateway's IPv6 address */ 22struct in6_addr net_gateway6 = ZERO_IPV6_ADDR; 23/* Our IPv6 addr (0 = unknown) */ 24struct in6_addr net_ip6 = ZERO_IPV6_ADDR; 25/* Our link local IPv6 addr (0 = unknown) */ 26struct in6_addr net_link_local_ip6 = ZERO_IPV6_ADDR; 27/* set server IPv6 addr (0 = unknown) */ 28struct in6_addr net_server_ip6 = ZERO_IPV6_ADDR; 29/* The prefix length of our network */ 30u32 net_prefix_length; 31 32bool use_ip6; 33 34static 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 61U_BOOT_ENV_CALLBACK(ip6addr, on_ip6addr); 62 63static 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 72U_BOOT_ENV_CALLBACK(gatewayip6, on_gatewayip6); 73 74static 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 83U_BOOT_ENV_CALLBACK(serverip6, on_serverip6); 84 85int 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 91int 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 97void 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 106void 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 116void 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 128void 129ip6_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 136int 137ip6_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 164static 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 178static 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 187static 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); 239out: 240 return result; 241} 242 243unsigned 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 254unsigned short int 255csum_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 308int 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 327int 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 380int 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} 450