stglibs/pinger.lib/pinger.cpp

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