stglibs/pinger.lib/pinger.cpp

   1 #include <stdlib.h>
   2 #include <pthread.h>
   3 #include <signal.h>
   4 #include <netdb.h>
   5 #include <sys/types.h>
   6 #include <sys/socket.h>
   7 #include <netinet/in.h>
   8 #include <arpa/inet.h>
   9 #include <sys/time.h>
  10 #include <string.h>
  11 #include <unistd.h>
  12 #include <fcntl.h>
  13 #include <errno.h>
  14 #include <math.h>
  15 #include <stdio.h>
  16
  17 #include "pinger.h"
  18 #include "stg/common.h"
  19 #include "stg/locker.h"
  20
  21 #ifdef STG_TIME
  22 extern volatile time_t stgTime;
  23 #endif
  24
  25 //-----------------------------------------------------------------------------
  26 STG_PINGER::STG_PINGER(time_t d)
  27     : delay(d),
  28       nonstop(false),
  29       isRunningRecver(false),
  30       isRunningSender(false),
  31       sendSocket(-1),
  32       recvSocket(-1),
  33       sendThread(),
  34       recvThread(),
  35       pmSend(),
  36       pid(0),
  37       errorStr(),
  38       pingIP(),
  39       ipToAdd(),
  40       ipToDel(),
  41       mutex()
  42 {
  43     pthread_mutex_init(&mutex, NULL);
  44     memset(&pmSend, 0, sizeof(pmSend));
  45 }
  46 //-----------------------------------------------------------------------------
  47 STG_PINGER::~STG_PINGER()
  48 {
  49     pthread_mutex_destroy(&mutex);
  50 }
  51 //-----------------------------------------------------------------------------
  52 int STG_PINGER::Start()
  53 {
  54     struct protoent *proto = NULL;
  55     proto = getprotobyname("ICMP");
  56     sendSocket = socket(PF_INET, SOCK_RAW, proto->p_proto);
  57     recvSocket = socket(PF_INET, SOCK_RAW, proto->p_proto);
  58     nonstop = true;
  59     pid = (int) getpid() % 65535;
  60     if (sendSocket < 0 || recvSocket < 0)
  61         {
  62         errorStr = "Cannot create socket.";
  63         return -1;
  64         }
  65
  66     if (pthread_create(&sendThread, NULL, RunSendPing, this))
  67         {
  68         errorStr = "Cannot create send thread.";
  69         return -1;
  70         }
  71
  72     if (pthread_create(&recvThread, NULL, RunRecvPing, this))
  73         {
  74         errorStr = "Cannot create recv thread.";
  75         return -1;
  76         }
  77
  78     return 0;
  79 }
  80 //-----------------------------------------------------------------------------
  81 int STG_PINGER::Stop()
  82 {
  83     close(recvSocket);
  84     nonstop = false;
  85     if (isRunningRecver)
  86         {
  87         //5 seconds to thread stops itself
  88         for (size_t i = 0; i < 25; i++)
  89             {
  90             if (i % 5 == 0)
  91                 SendPing(0x0100007f);//127.0.0.1
  92
  93             if (!isRunningRecver)
  94                 break;
  95
  96             struct timespec ts = {0, 200000000};
  97             nanosleep(&ts, NULL);
  98             }
  99
 100         //after 5 seconds waiting thread still running. now killing it
 101         if (isRunningRecver)
 102             {
 103             if (pthread_kill(recvThread, SIGINT))
 104                 {
 105                 errorStr = "Cannot kill thread.";
 106                 return -1;
 107                 }
 108             }
 109         }
 110
 111     if (isRunningSender)
 112         {
 113         //5 seconds to thread stops itself
 114         for (size_t i = 0; i < 25; i++)
 115             {
 116             if (!isRunningSender)
 117                 break;
 118
 119             struct timespec ts = {0, 200000000};
 120             nanosleep(&ts, NULL);
 121             }
 122
 123         //after 5 seconds waiting thread still running. now killing it
 124         if (isRunningSender)
 125             {
 126             if (pthread_kill(sendThread, SIGINT))
 127                 {
 128                 errorStr = "Cannot kill thread.";
 129                 return -1;
 130                 }
 131             }
 132         }
 133
 134     close(sendSocket);
 135     return 0;
 136 }
 137 //-----------------------------------------------------------------------------
 138 void STG_PINGER::AddIP(uint32_t ip)
 139 {
 140     STG_LOCKER lock(&mutex, __FILE__, __LINE__);
 141     ipToAdd.push_back(ip);
 142 }
 143 //-----------------------------------------------------------------------------
 144 void STG_PINGER::DelIP(uint32_t ip)
 145 {
 146     STG_LOCKER lock(&mutex, __FILE__, __LINE__);
 147     ipToDel.push_back(ip);
 148 }
 149 //-----------------------------------------------------------------------------
 150 void STG_PINGER::RealAddIP()
 151     {
 152     STG_LOCKER lock(&mutex, __FILE__, __LINE__);
 153
 154     std::list<uint32_t>::iterator iter;
 155     iter = ipToAdd.begin();
 156     while (iter != ipToAdd.end())
 157         {
 158         pingIP.insert(std::make_pair(*iter, 0));
 159         ++iter;
 160         }
 161     ipToAdd.erase(ipToAdd.begin(), ipToAdd.end());
 162     }
 163 //-----------------------------------------------------------------------------
 164 void STG_PINGER::RealDelIP()
 165 {
 166     STG_LOCKER lock(&mutex, __FILE__, __LINE__);
 167
 168     std::list<uint32_t>::iterator iter;
 169     std::multimap<uint32_t, time_t>::iterator treeIter;
 170     iter = ipToDel.begin();
 171     while (iter != ipToDel.end())
 172         {
 173         treeIter = pingIP.find(*iter);
 174         if (treeIter != pingIP.end())
 175             pingIP.erase(treeIter);
 176
 177         ++iter;
 178         }
 179     ipToDel.erase(ipToDel.begin(), ipToDel.end());
 180 }
 181 //-----------------------------------------------------------------------------
 182 int STG_PINGER::GetPingIPNum() const
 183 {
 184     return pingIP.size();
 185 }
 186 //-----------------------------------------------------------------------------
 187 void STG_PINGER::PrintAllIP()
 188 {
 189     STG_LOCKER lock(&mutex, __FILE__, __LINE__);
 190     std::multimap<uint32_t, time_t>::iterator iter;
 191     iter = pingIP.begin();
 192     while (iter != pingIP.end())
 193         {
 194         uint32_t ip = iter->first;
 195         time_t t = iter->second;
 196         std::string s;
 197         x2str(t, s);
 198         printf("ip = %s, time = %9s\n", inet_ntostring(ip).c_str(), s.c_str());
 199         ++iter;
 200         }
 201
 202 }
 203 //-----------------------------------------------------------------------------
 204 int STG_PINGER::GetIPTime(uint32_t ip, time_t * t) const
 205 {
 206     STG_LOCKER lock(&mutex, __FILE__, __LINE__);
 207     std::multimap<uint32_t, time_t>::const_iterator treeIter;
 208
 209     treeIter = pingIP.find(ip);
 210     if (treeIter == pingIP.end())
 211         return -1;
 212
 213     *t = treeIter->second;
 214     return 0;
 215 }
 216 //-----------------------------------------------------------------------------
 217 uint16_t STG_PINGER::PingCheckSum(void * data, int len)
 218 {
 219     unsigned short * buf = (unsigned short *)data;
 220     unsigned int sum = 0;
 221     unsigned short result;
 222
 223     for ( sum = 0; len > 1; len -= 2 )
 224         sum += *buf++;
 225
 226     if ( len == 1 )
 227         sum += *(unsigned char*)buf;
 228
 229     sum = (sum >> 16) + (sum & 0xFFFF);
 230     sum += (sum >> 16);
 231     result = ~sum;
 232     return result;
 233 }
 234 //-----------------------------------------------------------------------------
 235 int STG_PINGER::SendPing(uint32_t ip)
 236 {
 237     struct sockaddr_in addr;
 238     memset(&addr, 0, sizeof(addr));
 239     addr.sin_family = AF_INET;
 240     addr.sin_port = 0;
 241     addr.sin_addr.s_addr = ip;
 242
 243     memset(&pmSend, 0, sizeof(pmSend));
 244     pmSend.hdr.type = ICMP_ECHO;
 245     pmSend.hdr.un.echo.id = pid;
 246     memcpy(pmSend.msg, &ip, sizeof(ip));
 247
 248     pmSend.hdr.checksum = PingCheckSum(&pmSend, sizeof(pmSend));
 249
 250     if (sendto(sendSocket, &pmSend, sizeof(pmSend), 0, (sockaddr *)&addr, sizeof(addr)) <= 0 )
 251         {
 252         errorStr = "Send ping error: " + std::string(strerror(errno));
 253         return -1;
 254         }
 255
 256
 257     return 0;
 258 }
 259 //-----------------------------------------------------------------------------
 260 uint32_t STG_PINGER::RecvPing()
 261 {
 262     struct sockaddr_in addr;
 263     uint32_t ipAddr = 0;
 264
 265     char buf[128];
 266     memset(buf, 0, sizeof(buf));
 267     int bytes;
 268     socklen_t len = sizeof(addr);
 269
 270     bytes = recvfrom(recvSocket, &buf, sizeof(buf), 0, (struct sockaddr*)&addr, &len);
 271     if (bytes > 0)
 272         {
 273         struct IP_HDR * ip = (struct IP_HDR *)buf;
 274         struct ICMP_HDR *icmp = (struct ICMP_HDR *)(buf + ip->ihl * 4);
 275
 276         if (icmp->un.echo.id != pid)
 277             return 0;
 278
 279         ipAddr = *(uint32_t*)(buf + sizeof(ICMP_HDR) + ip->ihl * 4);
 280         }
 281
 282     return ipAddr;
 283 }
 284 //-----------------------------------------------------------------------------
 285 void * STG_PINGER::RunSendPing(void * d)
 286 {
 287     STG_PINGER * pinger = static_cast<STG_PINGER *>(d);
 288
 289     pinger->isRunningSender = true;
 290     time_t lastPing = 0;
 291     while (pinger->nonstop)
 292         {
 293         pinger->RealAddIP();
 294         pinger->RealDelIP();
 295
 296         std::multimap<uint32_t, time_t>::iterator iter;
 297         iter = pinger->pingIP.begin();
 298         while (iter != pinger->pingIP.end())
 299             {
 300             pinger->SendPing(iter->first);
 301             ++iter;
 302             }
 303
 304         time_t currTime;
 305
 306         #ifdef STG_TIME
 307         lastPing = stgTime;
 308         currTime = stgTime;
 309         #else
 310         currTime = lastPing = time(NULL);
 311         #endif
 312
 313         while (currTime - lastPing < pinger->delay && pinger->nonstop)
 314             {
 315             #ifdef STG_TIME
 316             currTime = stgTime;
 317             #else
 318             currTime = time(NULL);
 319             #endif
 320             struct timespec ts = {0, 20000000};
 321             nanosleep(&ts, NULL);
 322             }
 323         }
 324
 325     pinger->isRunningSender = false;
 326
 327     return NULL;
 328 }
 329 //-----------------------------------------------------------------------------
 330 void * STG_PINGER::RunRecvPing(void * d)
 331 {
 332     STG_PINGER * pinger = static_cast<STG_PINGER *>(d);
 333
 334     pinger->isRunningRecver = true;
 335
 336     while (pinger->nonstop)
 337         {
 338         uint32_t ip = pinger->RecvPing();
 339
 340         if (ip)
 341             {
 342             std::multimap<uint32_t, time_t>::iterator treeIterUpper = pinger->pingIP.upper_bound(ip);
 343             std::multimap<uint32_t, time_t>::iterator treeIterLower = pinger->pingIP.lower_bound(ip);
 344             while (treeIterUpper != treeIterLower)
 345                 {
 346                 #ifdef STG_TIME
 347                 treeIterLower->second = stgTime;
 348                 #else
 349                 treeIterLower->second = time(NULL);
 350                 #endif
 351                 ++treeIterLower;
 352                 }
 353             }
 354
 355         }
 356     pinger->isRunningRecver = false;
 357     return NULL;
 358 }
 359 //-----------------------------------------------------------------------------