PresenceDetection/WiFi/Functions.cpp

#include "Functions.h"
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/param.h>
#include <sys/socket.h>
#include <netinet/in_systm.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <unistd.h>
#include <string>
#include <stdexcept>


namespace PresenceDetection {
namespace WiFi {

#define	DEFDATALEN	(64-ICMP_MINLEN)
#define	MAXIPLEN	60
#define	MAXICMPLEN	76
#define	MAXPACKET	(65536 - 60 - ICMP_MINLEN)

bool Functions::Ping(const std::string& ipAddress)
{
	int s, i, cc, packlen, datalen = DEFDATALEN;
	struct hostent *hp;
	struct sockaddr_in to, from;
	struct ip *ip;
	u_char *packet, outpack[MAXPACKET];
	char hnamebuf[MAXHOSTNAMELEN];
	std::string hostname = ipAddress;
	struct icmp *icp;
	int ret, fromlen, hlen;
	fd_set rfds;
	struct timeval tv;
	int retval;

	to.sin_family = AF_INET;
	to.sin_addr.s_addr = inet_addr(ipAddress.c_str());

	packlen = datalen + MAXIPLEN + MAXICMPLEN;
	if ( (packet = (u_char *)malloc((u_int)packlen)) == NULL)
		throw std::runtime_error("Can't allocate Packet");

	if ( (s = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP)) < 0)
		throw std::runtime_error("Can't send on ICMP Socket");

	icp = (struct icmp*)outpack;
	icp->icmp_type = ICMP_ECHO;
	icp->icmp_code = 0;
	icp->icmp_cksum = 0;
	icp->icmp_seq = 12345;
	icp->icmp_id = getpid();

	cc = datalen + ICMP_MINLEN;
	icp->icmp_cksum = ICMPChecksum((unsigned short*)icp,cc);

	i = sendto(s, (char*)outpack, cc, 0, (struct sockaddr*)&to, (socklen_t)sizeof(struct sockaddr_in));
	if (i < 0)
		throw std::runtime_error("Can't send on ICMP Packet");

	FD_ZERO(&rfds);
	FD_SET(s, &rfds);
	tv.tv_sec = 0;
	tv.tv_usec = 500000;

	while(1)
	{
		retval = select(s+1, &rfds, NULL, NULL, &tv);
		if (retval == -1)
			throw std::runtime_error("Can't select file descriptor");

		if (!retval)
			return false;

		fromlen = sizeof(sockaddr_in);
		if ((ret = recvfrom(s, (char*)packet, packlen, 0,(struct sockaddr*)&from, (socklen_t*)&fromlen)) < 0)
			throw std::runtime_error("Can't receive message from socket");

		ip = (struct ip*)((char*)packet);
		hlen = sizeof(struct ip);
		if (ret < (hlen + ICMP_MINLEN))
			throw std::runtime_error("Can't allocate memory for receiving packets");

		icp = (struct icmp*)(packet + hlen);
		if (icp->icmp_type != ICMP_ECHOREPLY)
			continue;

		if (icp->icmp_seq != 12345 || icp->icmp_id != getpid())
			continue;

		return true;
	}

	return false;
}

uint16_t Functions::ICMPChecksum(uint16_t* addr, unsigned len)
{
  uint16_t answer = 0;
  uint32_t sum = 0;

  while (len > 1)
	{
    sum += *addr++;
    len -= 2;
  }

  if (len == 1)
	{
    *(unsigned char*)&answer = *(unsigned char*)addr ;
    sum += answer;
  }

  sum = (sum >> 16) + (sum & 0xffff);
  sum += (sum >> 16);
  answer = ~sum;
  return answer;
}

} // namespace WiFi
} // namespace PresenceDetection