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 <env_internal.h>
13 #include <malloc.h>
14 #include <net.h>
15 #include <net6.h>
16 #include <ndisc.h>
17 #include <vsprintf.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
on_ip6addr(const char * name,const char * value,enum env_op op,int flags)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
on_gatewayip6(const char * name,const char * value,enum env_op op,int flags)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
on_serverip6(const char * name,const char * value,enum env_op op,int flags)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
ip6_is_unspecified_addr(struct in6_addr * addr)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
ip6_is_our_addr(struct in6_addr * addr)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
ip6_make_eui(unsigned char eui[8],unsigned char const enetaddr[6])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
ip6_make_lladdr(struct in6_addr * lladr,unsigned char const enetaddr[6])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
ip6_make_snma(struct in6_addr * mcast_addr,struct in6_addr * ip6_addr)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
ip6_make_mult_ethdstaddr(unsigned char enetaddr[6],struct in6_addr * mcast_addr)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
ip6_addr_in_subnet(struct in6_addr * our_addr,struct in6_addr * neigh_addr,u32 plen)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
csum_fold(unsigned int sum)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
from32to16(unsigned int x)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
csum_do_csum(const u8 * buff,int len)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
csum_partial(const unsigned char * buff,int len,unsigned int sum)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
csum_ipv6_magic(struct in6_addr * saddr,struct in6_addr * daddr,u16 len,unsigned short proto,unsigned int csum)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
ip6_add_hdr(uchar * xip,struct in6_addr * src,struct in6_addr * dest,int nextheader,int hoplimit,int payload_len)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
net_send_udp_packet6(uchar * ether,struct in6_addr * dest,int dport,int sport,int len)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
net_ip6_handler(struct ethernet_hdr * et,struct ip6_hdr * ip6,int len)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 }
450