apps/streamer/arping.c

Go to the documentation of this file.

/*
 * Modified by Alexander Melichenko for using in Elphel cameras
 * 
 * arping.c - Ping hosts by ARP requests/replies
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Author:      Alexey Kuznetsov <kuznet@ms2.inr.ac.ru>
 * Busybox port: Nick Fedchik <nick@fedchik.org.ua>
 */

#include <sys/ioctl.h>
#include <sys/signal.h>
#include <sys/time.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <netinet/ether.h>
#include <netpacket/packet.h>

#include "arping.h"

static struct in_addr src;
static struct in_addr dst;
static struct sockaddr_ll me;
static struct sockaddr_ll he;
static int32_t s;

static int32_t send_pack(int32_t sock, struct in_addr *src_addr,
                         struct in_addr *dst_addr, struct sockaddr_ll *ME,
                         struct sockaddr_ll *HE)
{
    int32_t err;
    uint8_t buf[256];
    struct arphdr *ah = (struct arphdr *) buf;
    uint8_t *p = (uint8_t *) (ah + 1);

    ah->ar_hrd = htons(ME->sll_hatype);
    ah->ar_hrd = htons(ARPHRD_ETHER);
    ah->ar_pro = htons(ETH_P_IP);
    ah->ar_hln = ME->sll_halen;
    ah->ar_pln = 4;
    ah->ar_op = htons(ARPOP_REQUEST);

    memcpy(p, &ME->sll_addr, ah->ar_hln);
    p += ME->sll_halen;

    memcpy(p, src_addr, 4);
    p += 4;

    memcpy(p, &HE->sll_addr, ah->ar_hln);
    p += ah->ar_hln;

    memcpy(p, dst_addr, 4);
    p += 4;

    err = sendto(sock, buf, p - buf, 0, (struct sockaddr *) HE, sizeof(*HE));
    return err;
}


int32_t recv_pack(uint8_t * buf, int32_t len, struct sockaddr_ll * FROM,
                  uint8_t * mac)
{
    struct arphdr *ah = (struct arphdr *) buf;
    uint8_t *p = (uint8_t *) (ah + 1);
    struct in_addr src_ip, dst_ip;

    /* Filter out wild packets */
    if (FROM->sll_pkttype != PACKET_HOST &&
        FROM->sll_pkttype != PACKET_BROADCAST &&
        FROM->sll_pkttype != PACKET_MULTICAST)
        return 0;

    /* Only these types are recognised */
    if (ah->ar_op != htons(ARPOP_REQUEST)
        && ah->ar_op != htons(ARPOP_REPLY))
        return 0;

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

    /* Protocol must be IP. */
    if (ah->ar_pro != htons(ETH_P_IP))
        return 0;
    if (ah->ar_pln != 4)
        return 0;
    if (ah->ar_hln != me.sll_halen)
        return 0;
    if (len < sizeof(*ah) + 2 * (4 + ah->ar_hln))
        return 0;
    memcpy(&src_ip, p + ah->ar_hln, 4);
    memcpy(&dst_ip, p + ah->ar_hln + 4 + ah->ar_hln, 4);

    if (src_ip.s_addr != dst.s_addr)
        return 0;
    if (src.s_addr != dst_ip.s_addr)
        return 0;
    if (memcmp(p + ah->ar_hln + 4, &me.sll_addr, ah->ar_hln))
        return 0;

    memcpy(mac, p, 6);

    return 1;
}

int32_t arping(struct in_addr _src, struct in_addr _dst, uint8_t * mac)
{
    int32_t socket_errno;
    int32_t ifindex = 0;
    int8_t  *device = "eth0";

    dst = _dst;
    src = _src;

    s = socket(PF_PACKET, SOCK_DGRAM, 0);
    socket_errno = errno;

    if (s < 0) {
        fprintf(stderr, "socket");
        return -1;
    }

    {
        struct ifreq ifr;

        memset(&ifr, 0, sizeof(ifr));
        strncpy(ifr.ifr_name, device, IFNAMSIZ - 1);
        if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
            fprintf(stderr, "Interface %s not found", device);
            return -1;
        }
        ifindex = ifr.ifr_ifindex;

        if (ioctl(s, SIOCGIFFLAGS, (int8_t *) & ifr)) {
            fprintf(stderr, "SIOCGIFFLAGS");
            return -1;
        }
        if (!(ifr.ifr_flags & IFF_UP)) {
            fprintf(stderr, "Interface %s is down", device);
            return -1;
        }
        if (ifr.ifr_flags & (IFF_NOARP | IFF_LOOPBACK)) {
            fprintf(stderr, "Interface %s is not ARPable\n", device);
            return -1;
        }
    }

    me.sll_family = AF_PACKET;
    me.sll_ifindex = ifindex;
    me.sll_protocol = htons(ETH_P_ARP);
    if (bind(s, (struct sockaddr *) &me, sizeof(me)) == -1) {
        fprintf(stderr, "Error: bind\n");
        return -1;
    }

    {
        int32_t alen = sizeof(me);

        if (getsockname(s, (struct sockaddr *) &me, &alen) == -1) {
            fprintf(stderr, "Error: getsockname\n");
            return -1;
        }
    }

    if (me.sll_halen == 0) {
        fprintf(stderr,
                "Interface \"%s\" is not ARPable (no ll address)\n",
                device);
        return -1;
    }
    he = me;
    memset(he.sll_addr, -1, he.sll_halen);

    {
        int8_t packet[4096];
        struct sockaddr_ll from;
        int32_t alen = sizeof(from);
        int32_t cc = 0, res;
        struct timeval tm;
        int32_t timeout = 3;
        time_t prevTime;
        fd_set fdset;

        tm.tv_usec = 0;
        time(&prevTime);

        send_pack(s, &src, &dst, &me, &he);

        while (timeout > 0) {
            FD_ZERO(&fdset);
            FD_SET(s, &fdset);
            tm.tv_sec = timeout;
            res =
                select(s + 1, &fdset, (fd_set *) NULL, (fd_set *) NULL,
                       &tm);
            if (res < 0) {
                fprintf(stderr, "Error: select\n");
                return -1;
            }
            if (res == 0) {
                fprintf(stderr, "Error: host not responded\n");
                return -1;
            }

            if (FD_ISSET(s, &fdset)) {
                if ((cc = recvfrom(s, packet, sizeof(packet), 0,
                                   (struct sockaddr *) &from,
                                   &alen)) < 0) {
                    fprintf(stderr, "Error: recvfrom\n");
                    return -1;
                } else {
                    break;
                }
            }
            timeout -= time(NULL) - prevTime;
            time(&prevTime);
        }

        res = recv_pack(packet, cc, &from, mac);
        if (res != 1)
            return -1;
    }

    return 0;
}

---