Chinaunix首页 | 论坛 | 博客
  • 博客访问: 6034647
  • 博文数量: 579
  • 博客积分: 1548
  • 博客等级: 上尉
  • 技术积分: 16632
  • 用 户 组: 普通用户
  • 注册时间: 2012-12-12 15:29
个人简介

http://www.csdn.net/ http://www.arm.com/zh/ https://www.kernel.org/ http://www.linuxpk.com/ http://www.51develop.net/ http://linux.chinaitlab.com/ http://www.embeddedlinux.org.cn http://bbs.pediy.com/

文章分类

全部博文(579)

文章存档

2018年(18)

2015年(91)

2014年(159)

2013年(231)

2012年(80)

分类: 嵌入式

2015-01-26 16:05:09


点击(此处)折叠或打开

  1. #define _GNU_SOURCE
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include <net/if.h>
  5. #include <netinet/in.h>
  6. #include <sys/types.h>
  7. #include <sys/socket.h>
  8. #include <arpa/inet.h>
  9. #include <unistd.h>
  10. #include <fcntl.h>
  11. #include <netinet/ip_icmp.h>
  12. #include <time.h>
  13. #include <string.h>
  14. #include <errno.h>
  15. #include <signal.h>
  16. #include <netpacket/packet.h>
  17. #include <netinet/ether.h>
  18. #include <stdbool.h>
  19. #include <sys/ioctl.h>

  20. #define MAX_PINGCOUNT 3

  21. //#define DEBUG 1
  22. #ifdef DEBUG
  23. #define DEBUGP(format,args...) fprintf(stdout, format, ##args)
  24. #else
  25. #define DEBUGP(format,args...)
  26. #endif

  27. #define MONOTONIC_US() ((unsigned)monotonic_us())

  28. enum {
  29.     DAD = 1,
  30.     UNSOLICITED = 2,
  31.     ADVERT = 4,
  32.     QUIET = 8,
  33.     QUIT_ON_REPLY = 16,
  34.     BCAST_ONLY = 32,
  35.     UNICASTING = 64
  36. };

  37. struct globals {
  38.     struct in_addr src;
  39.     struct in_addr dst;
  40.     struct sockaddr_ll me;
  41.     struct sockaddr_ll he;
  42.     int sock_fd;

  43.     int count; // = -1;
  44.     unsigned last;
  45.     unsigned timeout_us;
  46.     unsigned start;

  47.     unsigned sent;
  48.     unsigned brd_sent;
  49.     unsigned received;
  50.     unsigned brd_recv;
  51.     unsigned req_recv;
  52. }G;

  53. /* NB: unaligned parameter should be a pointer, aligned one -
  54.  * a lvalue. This makes it more likely to not swap them by mistake
  55.  */
  56. #if defined(i386) || defined(__x86_64__)
  57. # define move_from_unaligned_int(v, intp) ((v) = *(int*)(intp))
  58. # define move_from_unaligned16(v, u16p) ((v) = *(uint16_t*)(u16p))
  59. # define move_from_unaligned32(v, u32p) ((v) = *(uint32_t*)(u32p))
  60. # define move_to_unaligned32(u32p, v) (*(uint32_t*)(u32p) = (v))
  61. /* #elif ... - add your favorite arch */
  62. #else
  63. /* performs reasonably well (gcc usually inlines memcpy here) */
  64. # define move_from_unaligned_int(v, intp) (memcpy(&(v), (intp), sizeof(int)))
  65. # define move_from_unaligned16(v, u16p) (memcpy(&(v), (u16p), 2))
  66. # define move_from_unaligned32(v, u32p) (memcpy(&(v), (u32p), 4))
  67. # define move_to_unaligned32(u32p, v) do { \
  68.     uint32_t __t = (v); \
  69.     memcpy((u32p), &__t, 4); \
  70. } while (0)
  71. #endif

  72. #define src (G.src )
  73. #define dst (G.dst )
  74. #define me (G.me )
  75. #define he (G.he )
  76. #define sock_fd (G.sock_fd )
  77. #define count (G.count )
  78. #define last (G.last )
  79. #define timeout_us (G.timeout_us)
  80. #define start (G.start )
  81. #define sent (G.sent )
  82. #define brd_sent (G.brd_sent )
  83. #define received (G.received )
  84. #define brd_recv (G.brd_recv )
  85. #define req_recv (G.req_recv )
  86. #define INIT_G() do { \
  87.     memset(&G, 0, sizeof(G)); \
  88.     count = -1; \
  89. } while (0)

  90. int quiet = 1;

  91. static int setsockopt_bindtodevice(int fd, const char *iface)
  92. {
  93.     int r;
  94.     struct ifreq ifr;
  95.     strncpy(ifr.ifr_name, iface, IFNAMSIZ);
  96.     /* NB: passing (iface, strlen(iface) + 1) does not
  97.      * (maybe it works on _some_ kernels, but not on 2.6.26)
  98.      * Actually, ifr_name is at offset 0, and in practice
  99.      * just giving char[IFNAMSIZ] instead of struct ifreq works too.
  100.      * But just in case it's not true on some obscure arch... */
  101.     r = setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr));
  102.     if (r)
  103.         return -1;
  104.     return r;
  105. }

  106. unsigned long long monotonic_us(void)
  107. {
  108.     struct timeval tv;
  109.     gettimeofday(&tv, NULL);
  110.     return tv.tv_sec * 1000000ULL + tv.tv_usec;
  111. }

  112. static int sigaction_set(int signum, const struct sigaction *act)
  113. {
  114.     return sigaction(signum, act, NULL);
  115. }

  116. static void signal_SA_RESTART_empty_mask(int sig, void (*handler)(int))
  117. {
  118.     struct sigaction sa;
  119.     memset(&sa, 0, sizeof(sa));
  120.     /*sigemptyset(&sa.sa_mask);*/
  121.     sa.sa_flags = SA_RESTART;
  122.     sa.sa_handler = handler;
  123.     sigaction_set(sig, &sa);
  124. }

  125. static int send_pack(struct in_addr *src_addr,
  126.             struct in_addr *dst_addr, struct sockaddr_ll *ME,
  127.             struct sockaddr_ll *HE)
  128. {
  129.     int err;
  130.     unsigned char buf[256];
  131.     struct arphdr *ah = (struct arphdr *) buf;
  132.     unsigned char *p = (unsigned char *) (ah + 1);

  133.     ah->ar_hrd = htons(ARPHRD_ETHER);
  134.     ah->ar_pro = htons(ETH_P_IP);
  135.     ah->ar_hln = ME->sll_halen;
  136.     ah->ar_pln = 4;
  137.     ah->ar_op = htons(ARPOP_REQUEST);

  138.     p = mempcpy(p, &ME->sll_addr, ah->ar_hln);
  139.     p = mempcpy(p, src_addr, 4);

  140.     p = mempcpy(p, &HE->sll_addr, ah->ar_hln);

  141.     p = mempcpy(p, dst_addr, 4);

  142.     err = sendto(sock_fd, buf, p - buf, 0, (struct sockaddr *) HE, sizeof(*HE));
  143.     if (err == p - buf) {
  144.         last = MONOTONIC_US();
  145.         sent++;
  146. /*
  147.         if (!(option_mask32 & UNICASTING))
  148.             brd_sent++;
  149. */
  150.     }
  151.     return err;
  152. }

  153. static void finish(void)
  154. {
  155.     if (!quiet) {
  156.         printf("Sent %u probe(s) (%u broadcast(s))\n"
  157.             "Received %u repl%s"
  158.             " (%u request(s), %u broadcast(s))\n",
  159.             sent, brd_sent,
  160.             received, (received == 1) ? "ies" : "y",
  161.             req_recv, brd_recv);
  162.     }

  163.     close(sock_fd);
  164.     sock_fd = -1;
  165. }

  166. static void catcher(void)
  167. {
  168.     unsigned now;

  169.     now = MONOTONIC_US();
  170.     if (start == 0)
  171.         start = now;

  172.     if (count == 0 || (timeout_us && (now - start) > timeout_us)){
  173.         finish();
  174.         return;
  175.     }

  176.     /* count < 0 means "infinite count" */
  177.     if (count > 0)
  178.         count--;

  179.     if (last == 0 || (now - last) > 500000) {
  180.         send_pack(&src, &dst, &me, &he);
  181.         if(count == 0){
  182.             finish();
  183.             return;
  184.         }
  185.     }

  186.     alarm(1);
  187. }

  188. static bool recv_pack(unsigned char *buf, int len, struct sockaddr_ll *FROM)
  189. {
  190.     struct arphdr *ah = (struct arphdr *) buf;
  191.     unsigned char *p = (unsigned char *) (ah + 1);
  192.     struct in_addr src_ip, dst_ip;
  193.     /* moves below assume in_addr is 4 bytes big, ensure that */
  194.     struct BUG_in_addr_must_be_4 {
  195.         char BUG_in_addr_must_be_4[
  196.             sizeof(struct in_addr) == 4 ? 1 : -1
  197.         ];
  198.         char BUG_s_addr_must_be_4[
  199.             sizeof(src_ip.s_addr) == 4 ? 1 : -1
  200.         ];
  201.     };

  202.     /* Filter out wild packets */
  203.     if (FROM->sll_pkttype != PACKET_HOST
  204.      && FROM->sll_pkttype != PACKET_BROADCAST
  205.      && FROM->sll_pkttype != PACKET_MULTICAST)
  206.         return false;

  207.     /* Only these types are recognized */
  208.     if (ah->ar_op != htons(ARPOP_REQUEST) && ah->ar_op != htons(ARPOP_REPLY))
  209.         return false;

  210.     /* ARPHRD check and this darned FDDI hack here :-( */
  211.     if (ah->ar_hrd != htons(FROM->sll_hatype)
  212.      && (FROM->sll_hatype != ARPHRD_FDDI || ah->ar_hrd != htons(ARPHRD_ETHER)))
  213.         return false;

  214.     /* Protocol must be IP. */
  215.     if (ah->ar_pro != htons(ETH_P_IP)
  216.      || (ah->ar_pln != 4)
  217.      || (ah->ar_hln != me.sll_halen)
  218.      || (len < (int)(sizeof(*ah) + 2 * (4 + ah->ar_hln))))
  219.         return false;

  220.     move_from_unaligned32(src_ip.s_addr, p + ah->ar_hln);
  221.     move_from_unaligned32(dst_ip.s_addr, p + ah->ar_hln + 4 + ah->ar_hln);

  222.     if (dst.s_addr != src_ip.s_addr)
  223.         return false;
  224.     if (1) {
  225.         if ((src.s_addr != dst_ip.s_addr)
  226.             || (memcmp(p + ah->ar_hln + 4, &me.sll_addr, ah->ar_hln)))
  227.             return false;
  228.     } else {
  229.         /* DAD packet was:
  230.          src_ip = 0 (or some src)
  231.          src_hw = ME
  232.          dst_ip = tested address
  233.          dst_hw = <unspec>

  234.          We fail, if receive request/reply with:
  235.          src_ip = tested_address
  236.          src_hw != ME
  237.          if src_ip in request was not zero, check
  238.          also that it matches to dst_ip, otherwise
  239.          dst_ip/dst_hw do not matter.
  240.          */
  241.         if ((memcmp(p, &me.sll_addr, me.sll_halen) == 0)
  242.          || (src.s_addr && src.s_addr != dst_ip.s_addr))
  243.             return false;
  244.     }
  245.     
  246.     if (!quiet) {
  247.         int s_printed = 0;

  248.         printf("%scast re%s from %s [%s]",
  249.             FROM->sll_pkttype == PACKET_HOST ? "Uni" : "Broad",
  250.             ah->ar_op == htons(ARPOP_REPLY) ? "ply" : "quest",
  251.             inet_ntoa(src_ip),
  252.             ether_ntoa((struct ether_addr *) p));
  253.         if (dst_ip.s_addr != src.s_addr) {
  254.             printf("for %s ", inet_ntoa(dst_ip));
  255.             s_printed = 1;
  256.         }
  257.         if (memcmp(p + ah->ar_hln + 4, me.sll_addr, ah->ar_hln)) {
  258.             if (!s_printed)
  259.                 printf("for ");
  260.             printf("[%s]",
  261.                 ether_ntoa((struct ether_addr *) p + ah->ar_hln + 4));
  262.         }

  263.         if (last) {
  264.             unsigned diff = MONOTONIC_US() - last;
  265.             printf(" %u.%03ums\n", diff / 1000, diff % 1000);
  266.         } else {
  267.             printf(" UNSOLICITED?\n");
  268.         }
  269.         fflush(NULL);
  270.     }
  271.     received++;
  272.     if (FROM->sll_pkttype != PACKET_HOST)
  273.         brd_recv++;
  274.     if (ah->ar_op == htons(ARPOP_REQUEST))
  275.         req_recv++;

  276.     return true;
  277. }

  278. static int getifindex(int sockfd, const char *device)
  279. {
  280.     struct ifreq ifr;
  281.     int ret;
  282.     
  283.     memset(&ifr, 0, sizeof(ifr));
  284.     strncpy(ifr.ifr_name, device, IFNAMSIZ);
  285.     ret = ioctl(sockfd, SIOCGIFINDEX, &ifr);
  286.     if(ret < 0) return -1;
  287.     
  288.     me.sll_ifindex = ifr.ifr_ifindex;
  289.     
  290.     if(ioctl(sockfd, SIOCGIFFLAGS, (char *) &ifr) < 0){
  291.         return -2;
  292.     }

  293.     if (!(ifr.ifr_flags & IFF_UP)) {
  294.         return -3;
  295.     }
  296.     if (ifr.ifr_flags & (IFF_NOARP | IFF_LOOPBACK)) {
  297.         return -4;
  298.     }

  299.     return 0;
  300. }

  301. static int setsaddr(const char *device)
  302. {
  303.     struct sockaddr_in saddr;
  304.     const int const_int_1 = 1;
  305.     int probe_fd = socket(AF_INET, SOCK_DGRAM, 0);

  306.     if(probe_fd < 0) return -1;
  307.             
  308.     if(setsockopt_bindtodevice(probe_fd, device) < 0){
  309.         close(probe_fd);
  310.         return -2;
  311.     }
  312.     memset(&saddr, 0, sizeof(saddr));
  313.     saddr.sin_family = AF_INET;

  314.     /* Find IP address on this iface */
  315.     socklen_t alen = sizeof(saddr);

  316.     saddr.sin_port = htons(1025);
  317.     saddr.sin_addr = dst;

  318.     if (setsockopt(probe_fd, SOL_SOCKET, SO_DONTROUTE, &const_int_1, sizeof(const_int_1)) == -1){
  319.         close(probe_fd);
  320.         return -3;
  321.     }
  322.     if(connect(probe_fd, (struct sockaddr *) &saddr, sizeof(saddr)) < 0){
  323.         close(probe_fd);
  324.         return -4;
  325.     }
  326.     getsockname(probe_fd, (struct sockaddr *) &saddr, &alen);

  327.     if (saddr.sin_family != AF_INET){
  328.         close(probe_fd);
  329.         return -5;
  330.     }
  331.     src = saddr.sin_addr;

  332.     close(probe_fd);
  333.     return 0;
  334. }

  335. int arping(const char *device, const char *daddr, int pingcount)
  336. {
  337.     struct sockaddr_in saddr;
  338.     struct ifreq ifr;
  339.     char *source = NULL;
  340.     const char *target;
  341.     unsigned char packet[4096];
  342.     socklen_t alen;
  343.     char *err_str;
  344.     int ret;
  345.     
  346.     INIT_G();

  347.     sock_fd = socket(AF_PACKET, SOCK_DGRAM, 0);
  348.     if(sock_fd < 0) return -1;
  349.     
  350.     count = pingcount;
  351.     timeout_us = 5 * 1000000 + 500000;
  352.     target = daddr;

  353.     if(getifindex(sock_fd, device) < 0){
  354.         close(sock_fd);
  355.         return -2;
  356.     }

  357.     dst.s_addr = inet_addr(target);

  358.     if(setsaddr(device) < 0){
  359.         close(sock_fd);
  360.         return -3;
  361.     }

  362.     me.sll_family = AF_PACKET;
  363.     me.sll_protocol = htons(ETH_P_ARP);
  364.     if(bind(sock_fd, (struct sockaddr *) &me, sizeof(me)) < 0){
  365.         close(sock_fd);
  366.         return -4;
  367.     }

  368.     alen = sizeof(me);
  369.     getsockname(sock_fd, (struct sockaddr *) &me, &alen);
  370.     if (me.sll_halen == 0) {
  371.         close(sock_fd);
  372.         return -5;
  373.     }
  374.     he = me;
  375.     memset(he.sll_addr, -1, he.sll_halen);

  376.     if(!quiet){
  377.         /* inet_ntoa uses static storage, can't use in same printf */
  378.         printf("ARPING to %s", inet_ntoa(dst));
  379.         printf(" from %s via %s\n", inet_ntoa(src), device);
  380.     }
  381.     
  382.     signal_SA_RESTART_empty_mask(SIGINT, (void (*)(int))finish);
  383.     signal_SA_RESTART_empty_mask(SIGALRM, (void (*)(int))catcher);

  384.     catcher();

  385.     while (1) {
  386.         sigset_t sset, osset;
  387.         struct sockaddr_ll from;
  388.         socklen_t alen = sizeof(from);
  389.         int cc;

  390.         if(count == 0 || sock_fd < 0) break;
  391.         cc = recvfrom(sock_fd, packet, sizeof(packet), 0, (struct sockaddr *) &from, &alen);
  392.         if (cc < 0) {
  393.             continue;
  394.         }

  395.         sigemptyset(&sset);
  396.         sigaddset(&sset, SIGALRM);
  397.         sigaddset(&sset, SIGINT);
  398.         sigprocmask(SIG_BLOCK, &sset, &osset);
  399.         recv_pack(packet, cc, &from);
  400.         sigprocmask(SIG_SETMASK, &osset, NULL);
  401.     }

  402.     /* 没有收到任何应答 */
  403.     if(received == 0) return -6;
  404.     
  405.     return 0;
  406. }

  407. main(int argc, char **argv)
  408. {
  409.     if (argc != 3){
  410.         return 1;
  411.     }
  412.     int reply;
  413.     reply = arping(argv[2], argv[1], MAX_PINGCOUNT);
  414.     if (reply == 0){
  415.         fprintf(stdout, "Connect\n");
  416.          return 0;
  417.     }else{
  418.         fprintf(stdout, "Disconect\n");
  419.          return 2;
  420.     }
  421. }

阅读(2461) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~