libs/pinger/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 "stg/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
 103 if (isRunningSender)
 104     {
 105     //5 seconds to thread stops itself
 106     for (size_t i = 0; i < 25; i++)
 107         {
 108         if (!isRunningSender)
 109             break;
 110
 111         struct timespec ts = {0, 200000000};
 112         nanosleep(&ts, NULL);
 113         }
 114     }
 115
 116 close(sendSocket);
 117
 118 if (isRunningSender || isRunningRecver)
 119     return -1;
 120
 121 return 0;
 122 }
 123 //-----------------------------------------------------------------------------
 124 void STG_PINGER::AddIP(uint32_t ip)
 125 {
 126 STG_LOCKER lock(&mutex);
 127 ipToAdd.push_back(ip);
 128 }
 129 //-----------------------------------------------------------------------------
 130 void STG_PINGER::DelIP(uint32_t ip)
 131 {
 132 STG_LOCKER lock(&mutex);
 133 ipToDel.push_back(ip);
 134 }
 135 //-----------------------------------------------------------------------------
 136 void STG_PINGER::RealAddIP()
 137 {
 138 STG_LOCKER lock(&mutex);
 139
 140 std::list<uint32_t>::iterator iter;
 141 iter = ipToAdd.begin();
 142 while (iter != ipToAdd.end())
 143     {
 144     pingIP.insert(std::make_pair(*iter, 0));
 145     ++iter;
 146     }
 147 ipToAdd.erase(ipToAdd.begin(), ipToAdd.end());
 148 }
 149 //-----------------------------------------------------------------------------
 150 void STG_PINGER::RealDelIP()
 151 {
 152 STG_LOCKER lock(&mutex);
 153
 154 std::list<uint32_t>::iterator iter;
 155 std::multimap<uint32_t, time_t>::iterator treeIter;
 156 iter = ipToDel.begin();
 157 while (iter != ipToDel.end())
 158     {
 159     treeIter = pingIP.find(*iter);
 160     if (treeIter != pingIP.end())
 161         pingIP.erase(treeIter);
 162
 163     ++iter;
 164     }
 165 ipToDel.erase(ipToDel.begin(), ipToDel.end());
 166 }
 167 //-----------------------------------------------------------------------------
 168 void STG_PINGER::PrintAllIP()
 169 {
 170 STG_LOCKER lock(&mutex);
 171 std::multimap<uint32_t, time_t>::iterator iter;
 172 iter = pingIP.begin();
 173 while (iter != pingIP.end())
 174     {
 175     uint32_t ip = iter->first;
 176     time_t t = iter->second;
 177     std::string s;
 178     x2str(t, s);
 179     printf("ip = %s, time = %9s\n", inet_ntostring(ip).c_str(), s.c_str());
 180     ++iter;
 181     }
 182
 183 }
 184 //-----------------------------------------------------------------------------
 185 int STG_PINGER::GetIPTime(uint32_t ip, time_t * t) const
 186 {
 187 STG_LOCKER lock(&mutex);
 188 std::multimap<uint32_t, time_t>::const_iterator treeIter;
 189
 190 treeIter = pingIP.find(ip);
 191 if (treeIter == pingIP.end())
 192     return -1;
 193
 194 *t = treeIter->second;
 195 return 0;
 196 }
 197 //-----------------------------------------------------------------------------
 198 uint16_t STG_PINGER::PingCheckSum(void * data, int len)
 199 {
 200 uint16_t * buf = static_cast<uint16_t *>(data);
 201 uint32_t sum = 0;
 202 uint32_t result;
 203
 204 for ( sum = 0; len > 1; len -= 2 )
 205     sum += *buf++;
 206
 207 if ( len == 1 )
 208     sum += *reinterpret_cast<uint8_t*>(buf);
 209
 210 sum = (sum >> 16) + (sum & 0xFFFF);
 211 sum += (sum >> 16);
 212 result = ~sum;
 213 return static_cast<uint16_t>(result);
 214 }
 215 //-----------------------------------------------------------------------------
 216 int STG_PINGER::SendPing(uint32_t ip)
 217 {
 218 struct sockaddr_in addr;
 219 memset(&addr, 0, sizeof(addr));
 220 addr.sin_family = AF_INET;
 221 addr.sin_port = 0;
 222 addr.sin_addr.s_addr = ip;
 223
 224 memset(&pmSend, 0, sizeof(pmSend));
 225 pmSend.hdr.type = ICMP_ECHO;
 226 pmSend.hdr.un.echo.id = static_cast<uint16_t>(pid);
 227 memcpy(pmSend.msg, &ip, sizeof(ip));
 228
 229 pmSend.hdr.checksum = PingCheckSum(&pmSend, sizeof(pmSend));
 230
 231 if (sendto(sendSocket, &pmSend, sizeof(pmSend), 0, (sockaddr *)&addr, sizeof(addr)) <= 0 )
 232     {
 233     errorStr = "Send ping error: " + std::string(strerror(errno));
 234     return -1;
 235     }
 236
 237
 238 return 0;
 239 }
 240 //-----------------------------------------------------------------------------
 241 uint32_t STG_PINGER::RecvPing()
 242 {
 243 struct sockaddr_in addr;
 244 uint32_t ipAddr = 0;
 245
 246 char buf[128];
 247 memset(buf, 0, sizeof(buf));
 248 socklen_t len = sizeof(addr);
 249
 250 if (recvfrom(recvSocket, &buf, sizeof(buf), 0, reinterpret_cast<struct sockaddr*>(&addr), &len))
 251     {
 252     struct IP_HDR * ip = static_cast<struct IP_HDR *>(static_cast<void *>(buf));
 253     struct ICMP_HDR *icmp = static_cast<struct ICMP_HDR *>(static_cast<void *>(buf + ip->ihl * 4));
 254
 255     if (icmp->un.echo.id != pid)
 256         return 0;
 257
 258     ipAddr = *static_cast<uint32_t*>(static_cast<void *>(buf + sizeof(ICMP_HDR) + ip->ihl * 4));
 259     }
 260
 261 return ipAddr;
 262 }
 263 //-----------------------------------------------------------------------------
 264 void * STG_PINGER::RunSendPing(void * d)
 265 {
 266 sigset_t signalSet;
 267 sigfillset(&signalSet);
 268 pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 269
 270 STG_PINGER * pinger = static_cast<STG_PINGER *>(d);
 271
 272 pinger->isRunningSender = true;
 273 time_t lastPing = 0;
 274 while (pinger->nonstop)
 275     {
 276     pinger->RealAddIP();
 277     pinger->RealDelIP();
 278
 279     std::multimap<uint32_t, time_t>::iterator iter;
 280     iter = pinger->pingIP.begin();
 281     while (iter != pinger->pingIP.end())
 282         {
 283         pinger->SendPing(iter->first);
 284         ++iter;
 285         }
 286
 287     time_t currTime;
 288
 289     #ifdef STG_TIME
 290     lastPing = stgTime;
 291     currTime = stgTime;
 292     #else
 293     currTime = lastPing = time(NULL);
 294     #endif
 295
 296     while (currTime - lastPing < pinger->delay && pinger->nonstop)
 297         {
 298         #ifdef STG_TIME
 299         currTime = stgTime;
 300         #else
 301         currTime = time(NULL);
 302         #endif
 303         struct timespec ts = {0, 20000000};
 304         nanosleep(&ts, NULL);
 305         }
 306     }
 307
 308 pinger->isRunningSender = false;
 309
 310 return NULL;
 311 }
 312 //-----------------------------------------------------------------------------
 313 void * STG_PINGER::RunRecvPing(void * d)
 314 {
 315 sigset_t signalSet;
 316 sigfillset(&signalSet);
 317 pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 318
 319 STG_PINGER * pinger = static_cast<STG_PINGER *>(d);
 320
 321 pinger->isRunningRecver = true;
 322
 323 while (pinger->nonstop)
 324     {
 325     uint32_t ip = pinger->RecvPing();
 326
 327     if (ip)
 328         {
 329         std::multimap<uint32_t, time_t>::iterator treeIterUpper = pinger->pingIP.upper_bound(ip);
 330         std::multimap<uint32_t, time_t>::iterator treeIterLower = pinger->pingIP.lower_bound(ip);
 331         while (treeIterUpper != treeIterLower)
 332             {
 333             #ifdef STG_TIME
 334             treeIterLower->second = stgTime;
 335             #else
 336             treeIterLower->second = time(NULL);
 337             #endif
 338             ++treeIterLower;
 339             }
 340         }
 341
 342     }
 343 pinger->isRunningRecver = false;
 344 return NULL;
 345 }
 346 //-----------------------------------------------------------------------------