ping6.c 2.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118
  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 ping6 implementation */
  10. #include <common.h>
  11. #include <net.h>
  12. #include <net6.h>
  13. #include "ndisc.h"
  14. static ushort seq_no;
  15. /* the ipv6 address to ping */
  16. struct in6_addr net_ping_ip6;
  17. int
  18. ip6_make_ping(uchar *eth_dst_addr, struct in6_addr *neigh_addr, uchar *pkt)
  19. {
  20. struct echo_msg *msg;
  21. u16 len;
  22. u16 csum_p;
  23. uchar *pkt_old = pkt;
  24. len = sizeof(struct echo_msg);
  25. pkt += net_set_ether(pkt, eth_dst_addr, PROT_IP6);
  26. pkt += ip6_add_hdr(pkt, &net_ip6, neigh_addr, PROT_ICMPV6,
  27. IPV6_NDISC_HOPLIMIT, len);
  28. /* ICMPv6 - Echo */
  29. msg = (struct echo_msg *)pkt;
  30. msg->icmph.icmp6_type = IPV6_ICMP_ECHO_REQUEST;
  31. msg->icmph.icmp6_code = 0;
  32. msg->icmph.icmp6_cksum = 0;
  33. msg->icmph.icmp6_identifier = 0;
  34. msg->icmph.icmp6_sequence = htons(seq_no++);
  35. msg->id = msg->icmph.icmp6_identifier; /* these seem redundant */
  36. msg->sequence = msg->icmph.icmp6_sequence;
  37. /* checksum */
  38. csum_p = csum_partial((u8 *)msg, len, 0);
  39. msg->icmph.icmp6_cksum = csum_ipv6_magic(&net_ip6, neigh_addr, len,
  40. PROT_ICMPV6, csum_p);
  41. pkt += len;
  42. return pkt - pkt_old;
  43. }
  44. int ping6_send(void)
  45. {
  46. uchar *pkt;
  47. static uchar mac[6];
  48. /* always send neighbor solicit */
  49. memcpy(mac, net_null_ethaddr, 6);
  50. net_nd_sol_packet_ip6 = net_ping_ip6;
  51. net_nd_packet_mac = mac;
  52. pkt = net_nd_tx_packet;
  53. pkt += ip6_make_ping(mac, &net_ping_ip6, pkt);
  54. /* size of the waiting packet */
  55. net_nd_tx_packet_size = (pkt - net_nd_tx_packet);
  56. /* and do the ARP request */
  57. net_nd_try = 1;
  58. net_nd_timer_start = get_timer(0);
  59. ndisc_request();
  60. return 1; /* waiting */
  61. }
  62. static void ping6_timeout(void)
  63. {
  64. eth_halt();
  65. net_set_state(NETLOOP_FAIL); /* we did not get the reply */
  66. }
  67. void ping6_start(void)
  68. {
  69. printf("Using %s device\n", eth_get_name());
  70. net_set_timeout_handler(10000UL, ping6_timeout);
  71. ping6_send();
  72. }
  73. int ping6_receive(struct ethernet_hdr *et, struct ip6_hdr *ip6, int len)
  74. {
  75. struct icmp6hdr *icmp =
  76. (struct icmp6hdr *)(((uchar *)ip6) + IP6_HDR_SIZE);
  77. struct in6_addr src_ip;
  78. switch (icmp->icmp6_type) {
  79. case IPV6_ICMP_ECHO_REPLY:
  80. src_ip = ip6->saddr;
  81. if (memcmp(&net_ping_ip6, &src_ip, sizeof(struct in6_addr)))
  82. return -EINVAL;
  83. net_set_state(NETLOOP_SUCCESS);
  84. break;
  85. case IPV6_ICMP_ECHO_REQUEST:
  86. /* ignore for now.... */
  87. debug("Got ICMPv6 ECHO REQUEST from %pI6c\n", &ip6->saddr);
  88. return -EINVAL;
  89. default:
  90. debug("Unexpected ICMPv6 type 0x%x\n", icmp->icmp6_type);
  91. return -EINVAL;
  92. }
  93. return 0;
  94. }