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