123 lines
2.9 KiB
C++
123 lines
2.9 KiB
C++
#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
|