blob: 75577bcea17b9d663fa0af157fbd63203c6be9c8 [file] [log] [blame]
Viacheslav Mitrofanov33b50662022-12-02 12:17:58 +03001// 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>
Viacheslav Mitrofanov1feb6972022-12-02 12:18:05 +030017#include <ndisc.h>
Viacheslav Mitrofanov33b50662022-12-02 12:17:58 +030018
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;
Viacheslav Mitrofanov43ab8ec2022-12-02 12:18:04 +030033
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, '/');
Viacheslav Mitrofanov43ab8ec2022-12-02 12:18:04 +030050
Sean Edmond796e5492023-01-06 14:22:55 -080051 if (mask) {
52 net_prefix_length = simple_strtoul(mask + 1, NULL, 10);
53 len = mask - value;
54 } else {
55 len = strlen(value);
56 }
Viacheslav Mitrofanov43ab8ec2022-12-02 12:18:04 +030057
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);
Viacheslav Mitrofanov1feb6972022-12-02 12:18:05 +030084
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) {
Viacheslav Mitrofanoveeb0a2c2022-12-02 12:18:08 +0300410 case IPV6_ICMP_ECHO_REQUEST:
411 case IPV6_ICMP_ECHO_REPLY:
412 ping6_receive(et, ip6, len);
413 break;
Viacheslav Mitrofanov1feb6972022-12-02 12:18:05 +0300414 case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
415 case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
416 ndisc_receive(et, ip6, len);
417 break;
418 default:
419 break;
420 }
421 break;
422 case IPPROTO_UDP:
423 udp = (struct udp_hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
424 csum = udp->udp_xsum;
425 hlen = ntohs(ip6->payload_len);
426 udp->udp_xsum = 0;
427 /* checksum */
428 csum_p = csum_partial((u8 *)udp, hlen, 0);
429 udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
430 hlen, IPPROTO_UDP, csum_p);
431
432 if (csum != udp->udp_xsum)
433 return -EINVAL;
434
435 /* IP header OK. Pass the packet to the current handler. */
436 net_get_udp_handler()((uchar *)ip6 + IP6_HDR_SIZE +
437 UDP_HDR_SIZE,
438 ntohs(udp->udp_dst),
439 zero_ip,
440 ntohs(udp->udp_src),
441 ntohs(udp->udp_len) - 8);
442 break;
443 default:
444 return -EINVAL;
445 }
446
447 return 0;
448}