gdhcp/ipv4ll.c

   1 /*
   2  *
   3  *  IPV4 Local Link library with GLib integration
   4  *
   5  *  Copyright (C) 2009-2010  Aldebaran Robotics. All rights reserved.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21 #include <string.h>
  22 #include <stdlib.h>
  23 #include <errno.h>
  24 #include <unistd.h>
  25
  26 #include <sys/time.h>
  27 #include <sys/socket.h>
  28 #include <sys/types.h>
  29 #include <netpacket/packet.h>
  30 #include <net/ethernet.h>
  31 #include <netinet/if_ether.h>
  32
  33 #include <arpa/inet.h>
  34
  35 #include <glib.h>
  36 #include "ipv4ll.h"
  37
  38 /**
  39  * Return a random link local IP
  40  */
  41 uint32_t ipv4ll_random_ip(int seed)
  42 {
  43         unsigned tmp;
  44
  45         if (seed)
  46                 srand(seed);
  47         else {
  48                 struct timeval tv;
  49                 gettimeofday(&tv, NULL);
  50                 srand(tv.tv_usec);
  51         }
  52         do {
  53                 tmp = rand();
  54                 tmp = tmp & IN_CLASSB_HOST;
  55         } while (tmp > (IN_CLASSB_HOST - 0x0200));
  56         return ((LINKLOCAL_ADDR + 0x0100) + tmp);
  57 }
  58
  59 /**
  60  * Return a random delay in range of zero to secs*1000
  61  */
  62 guint ipv4ll_random_delay_ms(guint secs)
  63 {
  64         struct timeval tv;
  65         guint tmp;
  66
  67         gettimeofday(&tv, NULL);
  68         srand(tv.tv_usec);
  69         tmp = rand();
  70         return tmp % (secs * 1000);
  71 }
  72
  73 int ipv4ll_send_arp_packet(uint8_t* source_eth, uint32_t source_ip,
  74                     uint32_t target_ip, int ifindex)
  75 {
  76         struct sockaddr_ll dest;
  77         struct ether_arp p;
  78         uint32_t ip_source;
  79         uint32_t ip_target;
  80         int fd, n;
  81
  82         fd = socket(PF_PACKET, SOCK_DGRAM | SOCK_CLOEXEC, htons(ETH_P_ARP));
  83         if (fd < 0)
  84                 return -errno;
  85
  86         memset(&dest, 0, sizeof(dest));
  87         memset(&p, 0, sizeof(p));
  88
  89         dest.sll_family = AF_PACKET;
  90         dest.sll_protocol = htons(ETH_P_ARP);
  91         dest.sll_ifindex = ifindex;
  92         dest.sll_halen = ETH_ALEN;
  93         memset(dest.sll_addr, 0xFF, ETH_ALEN);
  94         if (bind(fd, (struct sockaddr *)&dest, sizeof(dest)) < 0) {
  95                 close(fd);
  96                 return -errno;
  97         }
  98
  99         ip_source = htonl(source_ip);
 100         ip_target = htonl(target_ip);
 101         p.arp_hrd = htons(ARPHRD_ETHER);
 102         p.arp_pro = htons(ETHERTYPE_IP);
 103         p.arp_hln = ETH_ALEN;
 104         p.arp_pln = 4;
 105         p.arp_op = htons(ARPOP_REQUEST);
 106
 107         memcpy(&p.arp_sha, source_eth, ETH_ALEN);
 108         memcpy(&p.arp_spa, &ip_source, sizeof(p.arp_spa));
 109         memcpy(&p.arp_tpa, &ip_target, sizeof(p.arp_tpa));
 110
 111         n = sendto(fd, &p, sizeof(p), 0,
 112                (struct sockaddr*) &dest, sizeof(dest));
 113         if (n < 0)
 114                 return -errno;
 115
 116         close(fd);
 117
 118         return n;
 119 }
 120
 121 int ipv4ll_arp_socket(int ifindex)
 122 {
 123         int fd;
 124         struct sockaddr_ll sock;
 125         fd = socket(PF_PACKET, SOCK_DGRAM | SOCK_CLOEXEC, htons(ETH_P_ARP));
 126         if (fd < 0)
 127                 return fd;
 128
 129         sock.sll_family = AF_PACKET;
 130         sock.sll_protocol = htons(ETH_P_ARP);
 131         sock.sll_ifindex = ifindex;
 132
 133         if (bind(fd, (struct sockaddr *) &sock, sizeof(sock)) != 0) {
 134                 close(fd);
 135                 return -errno;
 136         }
 137
 138         return fd;
 139 }