Skip to content
Snippets Groups Projects
host_utils.c 11.1 KiB
Newer Older
  • Learn to ignore specific revisions
  • Nevadita's avatar
    Nevadita committed
    /*
     * host_utils.c -- utils for host
     *
     * Author: Alexey Kuznetsov <kuznet@ms2.inr.ac.ru>
     * Author: Sukru Senli sukru.senli@inteno.se
     * Author: Nevadita Chatterjee nevadita.chatterjee@iopsys.eu
     *
     * This program is free software; you can redistribute it and/or
     * modify it under the terms of the GNU General Public License
     * version 2 as published by the Free Software Foundation.
     *
     * 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/>.
     */
    
    #define _GNU_SOURCE
    #include "host_utils.h"
    
    #include <linux/netlink.h>
    #include <linux/rtnetlink.h>
    
    
    struct in_addr src;
    struct in_addr dst;
    struct sockaddr_ll me;
    struct sockaddr_ll he;
    int sock_fd;
    
    typedef struct nl_req_s nl_req_t;
    
    struct nl_req_s {
    	struct nlmsghdr hdr;
    	struct rtgenmsg gen;
    };
    
    static int
    send_pack(struct in_addr *src_addr, struct in_addr *dst_addr, struct sockaddr_ll *ME, struct sockaddr_ll *HE)
    {
    	int err;
    	unsigned char buf[256];
    	struct arphdr *ah = (struct arphdr *) buf;
    	unsigned char *p = (unsigned char *) (ah + 1);
    
    	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);
    
    	p = (unsigned char *)mempcpy((void *)p, (const void *)ME->sll_addr, (size_t)ah->ar_hln);
    	p = (unsigned char *)mempcpy((void *)p, (const void *)src_addr, 4);
    
    	p = (unsigned char *)mempcpy((void *)p, (const void *)HE->sll_addr, (size_t)ah->ar_hln);
    	p = (unsigned char *)mempcpy((void *)p, (const void *)dst_addr, 4);
    
    	err = sendto(sock_fd, buf, p - buf, 0, (struct sockaddr *) HE, sizeof(*HE));
    	return err;
    }
    
    static bool
    recv_pack(char *buf, int len, struct sockaddr_ll *FROM)
    {
    	struct arphdr *ah = (struct arphdr *) buf;
    	unsigned char *p = (unsigned char *) (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 false;
    
    	/* Only these types are recognized */
    	if (ah->ar_op != htons(ARPOP_REPLY))
    		return false;
    
    	/* 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 false;
    
    	/* Protocol must be IP. */
    	if (ah->ar_pro != htons(ETH_P_IP)
    	 || (ah->ar_pln != 4)
    	 || (ah->ar_hln != me.sll_halen)
    	 || (len < (int)(sizeof(*ah) + 2 * (4 + ah->ar_hln))))
    		return false;
    
    	(src_ip.s_addr) = *(uint32_t *)(p + ah->ar_hln);
    	(dst_ip.s_addr) = *(uint32_t *)(p + ah->ar_hln + 4 + ah->ar_hln);
    
    	if (dst.s_addr != src_ip.s_addr)
    		return false;
    
    	if ((src.s_addr != dst_ip.s_addr) || (memcmp(p + ah->ar_hln + 4, &me.sll_addr, ah->ar_hln)))
    		return false;
    
    	return true;
    }
    
    
    bool
    host_send_arping(const char *targetIP, const char *device, int toms, int is_recv)
    {
    	struct sockaddr_in saddr;
    	struct ifreq ifr;
    	int probe_fd;
    	char packet[64];
    	struct sockaddr_ll from;
    	struct timeval timeout;
    
    	dbg("Inside %s %d\n", __func__, __LINE__);
    
    	sock_fd = socket(AF_PACKET, SOCK_DGRAM, 0);
    
    	memset(&ifr, 0, sizeof(ifr));
    	strncpy(ifr.ifr_name, device, IF_NAMESIZE);
    
    	if (ioctl(sock_fd, SIOCGIFINDEX, &ifr, sizeof(ifr)) < 0) {
    		close(sock_fd);
    		return false;
    	}
    
    	me.sll_family = AF_PACKET;
    	me.sll_ifindex = ifr.ifr_ifindex;
    	me.sll_protocol = htons(ETH_P_ARP);
    	bind(sock_fd, (struct sockaddr *) &me, sizeof(me));
    
    	socklen_t mlen = sizeof(me);
    
    	getsockname(sock_fd, (struct sockaddr *) &me, &mlen);
    
    	he = me;
    	memset(he.sll_addr, -1, he.sll_halen);
    
    	inet_pton(AF_INET, targetIP, &(dst.s_addr));
    
    	/* Get the sender IP address */
    	probe_fd = socket(AF_INET, SOCK_DGRAM, 0);
    	setsockopt(probe_fd, SOL_SOCKET, SO_BINDTODEVICE, &ifr, sizeof(ifr));
    	memset(&saddr, 0, sizeof(saddr));
    	saddr.sin_family = AF_INET;
    	socklen_t slen = sizeof(saddr);
    
    	saddr.sin_port = htons(1025);
    	saddr.sin_addr = dst;
    	connect(probe_fd, (struct sockaddr *) &saddr, sizeof(saddr));
    	getsockname(probe_fd, (struct sockaddr *) &saddr, &slen);
    	src = saddr.sin_addr;
    	close(probe_fd);
    
    	send_pack(&src, &dst, &me, &he);
    
    	/*Here we are just sending the arp packet and returning*/
    
    	if (is_recv != 1) {
    		close(sock_fd);
    
    Nevadita's avatar
    Nevadita committed
    		return 1;
    
    Nevadita's avatar
    Nevadita committed
    
    	socklen_t alen = sizeof(from);
    	bool connected = false;
    	int cc = -1;
    
    	fd_set read_fds, write_fds, except_fds;
    
    	FD_ZERO(&read_fds);
    	FD_ZERO(&write_fds);
    	FD_ZERO(&except_fds);
    	FD_SET(sock_fd, &read_fds);
    
    
    	timeout.tv_sec = 0;
    	timeout.tv_usec = toms * 1000;
    
    	if (select(sock_fd + 1, &read_fds, &write_fds, &except_fds, &timeout) == 1)
    		cc = recvfrom(sock_fd, packet, sizeof(packet), 0, (struct sockaddr *) &from, &alen);
    
    	if (cc >= 0)
    		connected = recv_pack(packet, cc, &from);
    
    	close(sock_fd);
    	dbg("Inside %s %d connected %d\n", __func__, __LINE__, connected);
    	return connected;
    }
    
    int host_get_netlink_ip(uint8_t *mac_addr, char *ipv4_str, char *device)
    {
    	struct rtnl_neigh *neigh;
    	struct nl_object *nobj;
    	struct nl_cache *cache;
    	uint32_t ifindex = 0;
    	struct nl_sock *sk;
    	int i, num;
    	int ret;
    	char *ifname = NULL;
    
    	dbg("Inside %s %d\n", __func__, __LINE__);
    	sk = nl_socket_alloc();
    	if (!sk) {
    		err("Unable to open nl event socket\n");
    		return -1;
    	}
    
    	if (nl_connect(sk, NETLINK_ROUTE) < 0) {
    		nl_socket_free(sk);
    		return -1;
    	}
    
    	ret = rtnl_neigh_alloc_cache(sk, &cache);
    	if (ret) {
    		nl_socket_free(sk);
    		return -1;
    	}
    
    
    	num = nl_cache_nitems(cache);
    	nobj = nl_cache_get_first(cache);
    	neigh = (struct rtnl_neigh *)nobj;
    
    	for (i = 0; i < num; i++) {
    		struct nl_addr *lladdr;
    		struct nl_addr *ipaddr;
    		struct ip_address ip = {0};
    		uint8_t hwaddr[6] = {0};
    		char addr_str[24];
    
    		nl_object_get((struct nl_object *) neigh);
    
    		ifindex = rtnl_neigh_get_ifindex(neigh);
    		lladdr = rtnl_neigh_get_lladdr(neigh);
    		if (lladdr)
    			memcpy(hwaddr, nl_addr_get_binary_addr(lladdr),
    				nl_addr_get_len(lladdr));
    
    		if (hwaddr_is_zero(hwaddr)) {
    			nl_object_put((struct nl_object *) neigh);
    			nobj = nl_cache_get_next(nobj);
    			neigh = (struct rtnl_neigh *)nobj;
    			continue;
    		}
    
    		ipaddr = rtnl_neigh_get_dst(neigh);
    		if (ipaddr) {
    			ip.family = nl_addr_get_family(ipaddr);
    
    			if (ip.family == AF_INET6) {
    				nl_object_put((struct nl_object *) neigh);
    				nobj = nl_cache_get_next(nobj);
    				neigh = (struct rtnl_neigh *)nobj;
    				continue;
    			}
    			if (ip.family == AF_INET) {
    
    Nevadita's avatar
    Nevadita committed
    				memcpy(&ip.addr, nl_addr_get_binary_addr(ipaddr),
    				       nl_addr_get_len(ipaddr));
    			}
    		}
    
    		char *addr_s = nl_addr2str(ipaddr, addr_str, sizeof(addr_str));
    
    		if (addr_s == NULL) {
    			/* error */
    			err("nl_addr2str failed\n");
    
    			nl_object_put((struct nl_object *) neigh);
    
    			nl_cache_free(cache);
    			nl_socket_free(sk);
    
    Nevadita's avatar
    Nevadita committed
    			return -1;
    		}
    
    		ret = memcmp(hwaddr, mac_addr, 6);
    		if (ret == 0) {
    			strncpy(ipv4_str, addr_str, 24);
    			ifname = if_indextoname(ifindex, device);
    
    			if (ifname == NULL) {
    
    				nl_object_put((struct nl_object *) neigh);
    
    				nl_cache_free(cache);
    				nl_socket_free(sk);
    
    Nevadita's avatar
    Nevadita committed
    				return -1;
    
    Nevadita's avatar
    Nevadita committed
    			dbg("intfname = [%s] ipv4addr= [%s] mac=["MACFMT"]\n", device, ipv4_str, MAC2STR(hwaddr));
    
    			nl_object_put((struct nl_object *) neigh);
    
    Nevadita's avatar
    Nevadita committed
    			break;
    		}
    
    		nl_object_put((struct nl_object *) neigh);
    		nobj = nl_cache_get_next(nobj);
    		neigh = (struct rtnl_neigh *)nobj;
    	}
    
    	nl_cache_free(cache);
    	nl_socket_free(sk);
    
    	return 0;
    }
    
    int host_get_netlink_ip6(uint8_t *mac_addr, struct host_node *p)
    {
    	struct rtnl_neigh *neigh;
    	struct nl_object *nobj;
    	struct nl_cache *cache;
    	uint32_t ifindex = 0;
    	struct nl_sock *sk;
    	int i, num;
    	int ret;
    	char *ifname = NULL;
    
    	dbg("Inside %s %d\n", __func__, __LINE__);
    	sk = nl_socket_alloc();
    	if (!sk) {
    		err("Unable to open nl event socket\n");
    		return -1;
    	}
    
    	if (nl_connect(sk, NETLINK_ROUTE) < 0) {
    		nl_socket_free(sk);
    		return -1;
    	}
    
    	ret = rtnl_neigh_alloc_cache(sk, &cache);
    	if (ret) {
    		nl_socket_free(sk);
    		return -1;
    	}
    
    	num = nl_cache_nitems(cache);
    	nobj = nl_cache_get_first(cache);
    	neigh = (struct rtnl_neigh *)nobj;
    	p->ipv6addr_count = 0;
    
    	for (i = 0; i < num; i++) {
    		struct nl_addr *lladdr;
    		struct nl_addr *ipaddr;
    		struct ip_address ip = {0};
    		uint8_t hwaddr[6] = {0};
    		char addr_str[128];
    
    		nl_object_get((struct nl_object *) neigh);
    
    		ifindex = rtnl_neigh_get_ifindex(neigh);
    		lladdr = rtnl_neigh_get_lladdr(neigh);
    		if (lladdr)
    			memcpy(hwaddr, nl_addr_get_binary_addr(lladdr),
    				nl_addr_get_len(lladdr));
    
    		if (hwaddr_is_zero(hwaddr)) {
    			nl_object_put((struct nl_object *) neigh);
    			nobj = nl_cache_get_next(nobj);
    			neigh = (struct rtnl_neigh *)nobj;
    			continue;
    		}
    
    		ipaddr = rtnl_neigh_get_dst(neigh);
    		if (ipaddr) {
    			ip.family = nl_addr_get_family(ipaddr);
    			if (ip.family == AF_INET6) {
    				memcpy(&ip.addr, nl_addr_get_binary_addr(ipaddr),
    					nl_addr_get_len(ipaddr));
    			} else {
    				nl_object_put((struct nl_object *) neigh);
    				nobj = nl_cache_get_next(nobj);
    				neigh = (struct rtnl_neigh *)nobj;
    				continue;
    			}
    		}
    		char *addr_s = nl_addr2str(ipaddr, addr_str, sizeof(addr_str));
    
    		if (addr_s == NULL) {
    			/* error */
    			err("nl_addr2str failed\n");
    			nl_object_put((struct nl_object *) neigh);
    			nl_cache_free(cache);
    			nl_socket_free(sk);
    			return -1;
    		}
    
    		ret = memcmp(hwaddr, mac_addr, 6);
    		if (ret == 0) {
    			strncpy(p->ipv6addr[p->ipv6addr_count], addr_str, 128);
    			dbg("ipv6addr= [%s] mac=["MACFMT"]\n", p->ipv6addr[p->ipv6addr_count], MAC2STR(hwaddr));
    			p->ipv6addr_count = p->ipv6addr_count + 1;
    		}
    
    		nl_object_put((struct nl_object *) neigh);
    		nobj = nl_cache_get_next(nobj);
    		neigh = (struct rtnl_neigh *)nobj;
    	}
    
    	nl_cache_free(cache);
    	nl_socket_free(sk);
    
    	return 0;
    }
    
    
    void _E1B(uint8_t **packet_ppointer, uint8_t *memory_pointer)
    {
    	*memory_pointer = **packet_ppointer;
    	(*packet_ppointer) += 1;
    }
    
    // Extract/insert 2 bytes
    void _E2B(uint8_t **packet_ppointer, uint16_t *memory_pointer)
    {
    	uint16_t mem = 0;
    	*(((uint8_t *) &mem) + 0) = **packet_ppointer;
    	(*packet_ppointer)++;
    	*(((uint8_t *) &mem) + 1) = **packet_ppointer;
    	(*packet_ppointer)++;
    	*memory_pointer = ntohs(mem);
    }
    
    // Extract/insert N bytes (ignore endianess)
    void _EnB(uint8_t **packet_ppointer,
    		void *memory_pointer,
    		uint32_t n)
    {
    	memcpy(memory_pointer, *packet_ppointer, n);
    	(*packet_ppointer) += n;
    }
    
    // count includes end-of-message tlv
    uint8_t count_tlv_present_instream_unsafe(uint8_t *tlv_stream)
    {
    	uint8_t *p = tlv_stream;
    	uint8_t tlv_type, count = 0;
    	uint16_t tlv_len = 0;
    
    	if (tlv_stream == NULL) {
    		err("tlv Stream is empty\n");
    		return 0;
    	}
    
    	do {
    		_E1B(&p, &tlv_type);
    		_E2B(&p, &tlv_len);
    
    		p = p + tlv_len;
    		count++;
    	} while (tlv_type != TLV_TYPE_END_OF_MESSAGE);
    
    	return count;
    }
    
    uint16_t get_tlv_packet_len(uint8_t *tlv_stream)
    {
    	uint8_t *p;
    	uint8_t type;
    	uint16_t len;
    
    	if (tlv_stream == NULL)
    		return 0;
    
    	p = tlv_stream;
    	_E1B(&p, &type);
    	_E2B(&p, &len);
    
    	// tlv packet does not have length of tlv_type(1 Byte) and
    	// tlv_len(2 Bytes) included
    	return len + 3;
    }
    
    uint8_t get_tlv_packet_type(uint8_t *tlv_stream)
    {
    	uint8_t *p;
    	uint8_t type;
    	uint16_t len;
    
    	if (tlv_stream == NULL)
    		return 0;
    
    	p = tlv_stream;
    	_E1B(&p, &type);
    	_E2B(&p, &len);
    
    	return type;
    }