net6.c 10 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449
  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. /* Simple IPv6 network layer implementation */
  10. #include <common.h>
  11. #include <env_internal.h>
  12. #include <malloc.h>
  13. #include <net.h>
  14. #include <net6.h>
  15. #include <ndisc.h>
  16. /* NULL IPv6 address */
  17. struct in6_addr const net_null_addr_ip6 = ZERO_IPV6_ADDR;
  18. /* Our gateway's IPv6 address */
  19. struct in6_addr net_gateway6 = ZERO_IPV6_ADDR;
  20. /* Our IPv6 addr (0 = unknown) */
  21. struct in6_addr net_ip6 = ZERO_IPV6_ADDR;
  22. /* Our link local IPv6 addr (0 = unknown) */
  23. struct in6_addr net_link_local_ip6 = ZERO_IPV6_ADDR;
  24. /* set server IPv6 addr (0 = unknown) */
  25. struct in6_addr net_server_ip6 = ZERO_IPV6_ADDR;
  26. /* The prefix length of our network */
  27. u32 net_prefix_length;
  28. bool use_ip6;
  29. static int on_ip6addr(const char *name, const char *value, enum env_op op,
  30. int flags)
  31. {
  32. char *mask;
  33. size_t len;
  34. if (flags & H_PROGRAMMATIC)
  35. return 0;
  36. if (op == env_op_delete) {
  37. net_prefix_length = 0;
  38. net_copy_ip6(&net_ip6, &net_null_addr_ip6);
  39. return 0;
  40. }
  41. mask = strchr(value, '/');
  42. if (mask) {
  43. net_prefix_length = simple_strtoul(mask + 1, NULL, 10);
  44. len = mask - value;
  45. } else {
  46. len = strlen(value);
  47. }
  48. return string_to_ip6(value, len, &net_ip6);
  49. }
  50. U_BOOT_ENV_CALLBACK(ip6addr, on_ip6addr);
  51. static int on_gatewayip6(const char *name, const char *value, enum env_op op,
  52. int flags)
  53. {
  54. if (flags & H_PROGRAMMATIC)
  55. return 0;
  56. return string_to_ip6(value, strlen(value), &net_gateway6);
  57. }
  58. U_BOOT_ENV_CALLBACK(gatewayip6, on_gatewayip6);
  59. static int on_serverip6(const char *name, const char *value, enum env_op op,
  60. int flags)
  61. {
  62. if (flags & H_PROGRAMMATIC)
  63. return 0;
  64. return string_to_ip6(value, strlen(value), &net_server_ip6);
  65. }
  66. U_BOOT_ENV_CALLBACK(serverip6, on_serverip6);
  67. int ip6_is_unspecified_addr(struct in6_addr *addr)
  68. {
  69. return !(addr->s6_addr32[0] | addr->s6_addr32[1] |
  70. addr->s6_addr32[2] | addr->s6_addr32[3]);
  71. }
  72. int ip6_is_our_addr(struct in6_addr *addr)
  73. {
  74. return !memcmp(addr, &net_link_local_ip6, sizeof(struct in6_addr)) ||
  75. !memcmp(addr, &net_ip6, sizeof(struct in6_addr));
  76. }
  77. void ip6_make_eui(unsigned char eui[8], unsigned char const enetaddr[6])
  78. {
  79. memcpy(eui, enetaddr, 3);
  80. memcpy(&eui[5], &enetaddr[3], 3);
  81. eui[3] = 0xff;
  82. eui[4] = 0xfe;
  83. eui[0] ^= 2; /* "u" bit set to indicate global scope */
  84. }
  85. void ip6_make_lladdr(struct in6_addr *lladr, unsigned char const enetaddr[6])
  86. {
  87. unsigned char eui[8];
  88. memset(lladr, 0, sizeof(struct in6_addr));
  89. lladr->s6_addr16[0] = htons(IPV6_LINK_LOCAL_PREFIX);
  90. ip6_make_eui(eui, enetaddr);
  91. memcpy(&lladr->s6_addr[8], eui, 8);
  92. }
  93. void ip6_make_snma(struct in6_addr *mcast_addr, struct in6_addr *ip6_addr)
  94. {
  95. memset(mcast_addr, 0, sizeof(struct in6_addr));
  96. mcast_addr->s6_addr[0] = 0xff;
  97. mcast_addr->s6_addr[1] = IPV6_ADDRSCOPE_LINK;
  98. mcast_addr->s6_addr[11] = 0x01;
  99. mcast_addr->s6_addr[12] = 0xff;
  100. mcast_addr->s6_addr[13] = ip6_addr->s6_addr[13];
  101. mcast_addr->s6_addr[14] = ip6_addr->s6_addr[14];
  102. mcast_addr->s6_addr[15] = ip6_addr->s6_addr[15];
  103. }
  104. void
  105. ip6_make_mult_ethdstaddr(unsigned char enetaddr[6], struct in6_addr *mcast_addr)
  106. {
  107. enetaddr[0] = 0x33;
  108. enetaddr[1] = 0x33;
  109. memcpy(&enetaddr[2], &mcast_addr->s6_addr[12], 4);
  110. }
  111. int
  112. ip6_addr_in_subnet(struct in6_addr *our_addr, struct in6_addr *neigh_addr,
  113. u32 plen)
  114. {
  115. __be32 *addr_dwords;
  116. __be32 *neigh_dwords;
  117. addr_dwords = our_addr->s6_addr32;
  118. neigh_dwords = neigh_addr->s6_addr32;
  119. while (plen > 32) {
  120. if (*addr_dwords++ != *neigh_dwords++)
  121. return 0;
  122. plen -= 32;
  123. }
  124. /* Check any remaining bits */
  125. if (plen > 0) {
  126. if ((*addr_dwords >> (32 - plen)) !=
  127. (*neigh_dwords >> (32 - plen))) {
  128. return 0;
  129. }
  130. }
  131. return 1;
  132. }
  133. static inline unsigned int csum_fold(unsigned int sum)
  134. {
  135. sum = (sum & 0xffff) + (sum >> 16);
  136. sum = (sum & 0xffff) + (sum >> 16);
  137. /* Opaque moment. If reverse it to zero it will not be checked on
  138. * receiver's side. It leads to bad negibour advertisement.
  139. */
  140. if (sum == 0xffff)
  141. return sum;
  142. return ~sum;
  143. }
  144. static inline unsigned short from32to16(unsigned int x)
  145. {
  146. /* add up 16-bit and 16-bit for 16+c bit */
  147. x = (x & 0xffff) + (x >> 16);
  148. /* add up carry.. */
  149. x = (x & 0xffff) + (x >> 16);
  150. return x;
  151. }
  152. static u32 csum_do_csum(const u8 *buff, int len)
  153. {
  154. int odd;
  155. unsigned int result = 0;
  156. if (len <= 0)
  157. goto out;
  158. odd = 1 & (unsigned long)buff;
  159. if (odd) {
  160. #ifdef __LITTLE_ENDIAN
  161. result += (*buff << 8);
  162. #else
  163. result = *buff;
  164. #endif
  165. len--;
  166. buff++;
  167. }
  168. if (len >= 2) {
  169. if (2 & (unsigned long)buff) {
  170. result += *(unsigned short *)buff;
  171. len -= 2;
  172. buff += 2;
  173. }
  174. if (len >= 4) {
  175. const unsigned char *end = buff + ((u32)len & ~3);
  176. unsigned int carry = 0;
  177. do {
  178. unsigned int w = *(unsigned int *)buff;
  179. buff += 4;
  180. result += carry;
  181. result += w;
  182. carry = (w > result);
  183. } while (buff < end);
  184. result += carry;
  185. result = (result & 0xffff) + (result >> 16);
  186. }
  187. if (len & 2) {
  188. result += *(unsigned short *)buff;
  189. buff += 2;
  190. }
  191. }
  192. if (len & 1)
  193. #ifdef __LITTLE_ENDIAN
  194. result += *buff;
  195. #else
  196. result += (*buff << 8);
  197. #endif
  198. result = from32to16(result);
  199. if (odd)
  200. result = ((result >> 8) & 0xff) | ((result & 0xff) << 8);
  201. out:
  202. return result;
  203. }
  204. unsigned int csum_partial(const unsigned char *buff, int len, unsigned int sum)
  205. {
  206. unsigned int result = csum_do_csum(buff, len);
  207. /* add in old sum, and carry.. */
  208. result += sum;
  209. /* 16+c bits -> 16 bits */
  210. result = (result & 0xffff) + (result >> 16);
  211. return result;
  212. }
  213. unsigned short int
  214. csum_ipv6_magic(struct in6_addr *saddr, struct in6_addr *daddr, u16 len,
  215. unsigned short proto, unsigned int csum)
  216. {
  217. int carry;
  218. u32 ulen;
  219. u32 uproto;
  220. u32 sum = csum;
  221. sum += saddr->s6_addr32[0];
  222. carry = (sum < saddr->s6_addr32[0]);
  223. sum += carry;
  224. sum += saddr->s6_addr32[1];
  225. carry = (sum < saddr->s6_addr32[1]);
  226. sum += carry;
  227. sum += saddr->s6_addr32[2];
  228. carry = (sum < saddr->s6_addr32[2]);
  229. sum += carry;
  230. sum += saddr->s6_addr32[3];
  231. carry = (sum < saddr->s6_addr32[3]);
  232. sum += carry;
  233. sum += daddr->s6_addr32[0];
  234. carry = (sum < daddr->s6_addr32[0]);
  235. sum += carry;
  236. sum += daddr->s6_addr32[1];
  237. carry = (sum < daddr->s6_addr32[1]);
  238. sum += carry;
  239. sum += daddr->s6_addr32[2];
  240. carry = (sum < daddr->s6_addr32[2]);
  241. sum += carry;
  242. sum += daddr->s6_addr32[3];
  243. carry = (sum < daddr->s6_addr32[3]);
  244. sum += carry;
  245. ulen = htonl((u32)len);
  246. sum += ulen;
  247. carry = (sum < ulen);
  248. sum += carry;
  249. uproto = htonl(proto);
  250. sum += uproto;
  251. carry = (sum < uproto);
  252. sum += carry;
  253. return csum_fold(sum);
  254. }
  255. int ip6_add_hdr(uchar *xip, struct in6_addr *src, struct in6_addr *dest,
  256. int nextheader, int hoplimit, int payload_len)
  257. {
  258. struct ip6_hdr *ip6 = (struct ip6_hdr *)xip;
  259. ip6->version = 6;
  260. ip6->priority = 0;
  261. ip6->flow_lbl[0] = 0;
  262. ip6->flow_lbl[1] = 0;
  263. ip6->flow_lbl[2] = 0;
  264. ip6->payload_len = htons(payload_len);
  265. ip6->nexthdr = nextheader;
  266. ip6->hop_limit = hoplimit;
  267. net_copy_ip6(&ip6->saddr, src);
  268. net_copy_ip6(&ip6->daddr, dest);
  269. return sizeof(struct ip6_hdr);
  270. }
  271. int net_send_udp_packet6(uchar *ether, struct in6_addr *dest, int dport,
  272. int sport, int len)
  273. {
  274. uchar *pkt;
  275. struct udp_hdr *udp;
  276. u16 csum_p;
  277. udp = (struct udp_hdr *)((uchar *)net_tx_packet + net_eth_hdr_size() +
  278. IP6_HDR_SIZE);
  279. udp->udp_dst = htons(dport);
  280. udp->udp_src = htons(sport);
  281. udp->udp_len = htons(len + UDP_HDR_SIZE);
  282. /* checksum */
  283. udp->udp_xsum = 0;
  284. csum_p = csum_partial((u8 *)udp, len + UDP_HDR_SIZE, 0);
  285. udp->udp_xsum = csum_ipv6_magic(&net_ip6, dest, len + UDP_HDR_SIZE,
  286. IPPROTO_UDP, csum_p);
  287. /* if MAC address was not discovered yet, save the packet and do
  288. * neighbour discovery
  289. */
  290. if (!memcmp(ether, net_null_ethaddr, 6)) {
  291. net_copy_ip6(&net_nd_sol_packet_ip6, dest);
  292. net_nd_packet_mac = ether;
  293. pkt = net_nd_tx_packet;
  294. pkt += net_set_ether(pkt, net_nd_packet_mac, PROT_IP6);
  295. pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
  296. len + UDP_HDR_SIZE);
  297. memcpy(pkt, (uchar *)udp, len + UDP_HDR_SIZE);
  298. /* size of the waiting packet */
  299. net_nd_tx_packet_size = (pkt - net_nd_tx_packet) +
  300. UDP_HDR_SIZE + len;
  301. /* and do the neighbor solicitation */
  302. net_nd_try = 1;
  303. net_nd_timer_start = get_timer(0);
  304. ndisc_request();
  305. return 1; /* waiting */
  306. }
  307. pkt = (uchar *)net_tx_packet;
  308. pkt += net_set_ether(pkt, ether, PROT_IP6);
  309. pkt += ip6_add_hdr(pkt, &net_ip6, dest, IPPROTO_UDP, 64,
  310. len + UDP_HDR_SIZE);
  311. (void)eth_send(net_tx_packet, pkt - net_tx_packet + UDP_HDR_SIZE + len);
  312. return 0; /* transmitted */
  313. }
  314. int net_ip6_handler(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
  315. {
  316. struct in_addr zero_ip = {.s_addr = 0 };
  317. struct icmp6hdr *icmp;
  318. struct udp_hdr *udp;
  319. u16 csum;
  320. u16 csum_p;
  321. u16 hlen;
  322. if (len < IP6_HDR_SIZE)
  323. return -EINVAL;
  324. if (ip6->version != 6)
  325. return -EINVAL;
  326. switch (ip6->nexthdr) {
  327. case PROT_ICMPV6:
  328. icmp = (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
  329. csum = icmp->icmp6_cksum;
  330. hlen = ntohs(ip6->payload_len);
  331. icmp->icmp6_cksum = 0;
  332. /* checksum */
  333. csum_p = csum_partial((u8 *)icmp, hlen, 0);
  334. icmp->icmp6_cksum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
  335. hlen, PROT_ICMPV6, csum_p);
  336. if (icmp->icmp6_cksum != csum)
  337. return -EINVAL;
  338. switch (icmp->icmp6_type) {
  339. case IPV6_ICMP_ECHO_REQUEST:
  340. case IPV6_ICMP_ECHO_REPLY:
  341. ping6_receive(et, ip6, len);
  342. break;
  343. case IPV6_NDISC_NEIGHBOUR_SOLICITATION:
  344. case IPV6_NDISC_NEIGHBOUR_ADVERTISEMENT:
  345. case IPV6_NDISC_ROUTER_ADVERTISEMENT:
  346. ndisc_receive(et, ip6, len);
  347. break;
  348. default:
  349. break;
  350. }
  351. break;
  352. case IPPROTO_UDP:
  353. udp = (struct udp_hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
  354. csum = udp->udp_xsum;
  355. hlen = ntohs(ip6->payload_len);
  356. udp->udp_xsum = 0;
  357. /* checksum */
  358. csum_p = csum_partial((u8 *)udp, hlen, 0);
  359. udp->udp_xsum = csum_ipv6_magic(&ip6->saddr, &ip6->daddr,
  360. hlen, IPPROTO_UDP, csum_p);
  361. if (csum != udp->udp_xsum)
  362. return -EINVAL;
  363. /* IP header OK. Pass the packet to the current handler. */
  364. net_get_udp_handler()((uchar *)ip6 + IP6_HDR_SIZE +
  365. UDP_HDR_SIZE,
  366. ntohs(udp->udp_dst),
  367. zero_ip,
  368. ntohs(udp->udp_src),
  369. ntohs(udp->udp_len) - 8);
  370. break;
  371. default:
  372. return -EINVAL;
  373. }
  374. return 0;
  375. }