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 = std::to_string(t);
 178     printf("ip = %s, time = %9s\n", inet_ntostring(ip).c_str(), s.c_str());
 179     ++iter;
 180     }
 181
 182 }
 183 //-----------------------------------------------------------------------------
 184 int STG_PINGER::GetIPTime(uint32_t ip, time_t * t) const
 185 {
 186 STG_LOCKER lock(&mutex);
 187 std::multimap<uint32_t, time_t>::const_iterator treeIter;
 188
 189 treeIter = pingIP.find(ip);
 190 if (treeIter == pingIP.end())
 191     return -1;
 192
 193 *t = treeIter->second;
 194 return 0;
 195 }
 196 //-----------------------------------------------------------------------------
 197 uint16_t STG_PINGER::PingCheckSum(void * data, int len)
 198 {
 199 uint16_t * buf = static_cast<uint16_t *>(data);
 200 uint32_t sum = 0;
 201 uint32_t result;
 202
 203 for ( sum = 0; len > 1; len -= 2 )
 204     sum += *buf++;
 205
 206 if ( len == 1 )
 207     sum += *reinterpret_cast<uint8_t*>(buf);
 208
 209 sum = (sum >> 16) + (sum & 0xFFFF);
 210 sum += (sum >> 16);
 211 result = ~sum;
 212 return static_cast<uint16_t>(result);
 213 }
 214 //-----------------------------------------------------------------------------
 215 int STG_PINGER::SendPing(uint32_t ip)
 216 {
 217 struct sockaddr_in addr;
 218 memset(&addr, 0, sizeof(addr));
 219 addr.sin_family = AF_INET;
 220 addr.sin_port = 0;
 221 addr.sin_addr.s_addr = ip;
 222
 223 memset(&pmSend, 0, sizeof(pmSend));
 224 pmSend.hdr.type = ICMP_ECHO;
 225 pmSend.hdr.un.echo.id = static_cast<uint16_t>(pid);
 226 memcpy(pmSend.msg, &ip, sizeof(ip));
 227
 228 pmSend.hdr.checksum = PingCheckSum(&pmSend, sizeof(pmSend));
 229
 230 if (sendto(sendSocket, &pmSend, sizeof(pmSend), 0, (sockaddr *)&addr, sizeof(addr)) <= 0 )
 231     {
 232     errorStr = "Send ping error: " + std::string(strerror(errno));
 233     return -1;
 234     }
 235
 236
 237 return 0;
 238 }
 239 //-----------------------------------------------------------------------------
 240 uint32_t STG_PINGER::RecvPing()
 241 {
 242 struct sockaddr_in addr;
 243 uint32_t ipAddr = 0;
 244
 245 char buf[128];
 246 memset(buf, 0, sizeof(buf));
 247 socklen_t len = sizeof(addr);
 248
 249 if (recvfrom(recvSocket, &buf, sizeof(buf), 0, reinterpret_cast<struct sockaddr*>(&addr), &len))
 250     {
 251     struct IP_HDR * ip = static_cast<struct IP_HDR *>(static_cast<void *>(buf));
 252     struct ICMP_HDR *icmp = static_cast<struct ICMP_HDR *>(static_cast<void *>(buf + ip->ihl * 4));
 253
 254     if (icmp->un.echo.id != pid)
 255         return 0;
 256
 257     ipAddr = *static_cast<uint32_t*>(static_cast<void *>(buf + sizeof(ICMP_HDR) + ip->ihl * 4));
 258     }
 259
 260 return ipAddr;
 261 }
 262 //-----------------------------------------------------------------------------
 263 void * STG_PINGER::RunSendPing(void * d)
 264 {
 265 sigset_t signalSet;
 266 sigfillset(&signalSet);
 267 pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 268
 269 STG_PINGER * pinger = static_cast<STG_PINGER *>(d);
 270
 271 pinger->isRunningSender = true;
 272 time_t lastPing = 0;
 273 while (pinger->nonstop)
 274     {
 275     pinger->RealAddIP();
 276     pinger->RealDelIP();
 277
 278     std::multimap<uint32_t, time_t>::iterator iter;
 279     iter = pinger->pingIP.begin();
 280     while (iter != pinger->pingIP.end())
 281         {
 282         pinger->SendPing(iter->first);
 283         ++iter;
 284         }
 285
 286     time_t currTime;
 287
 288     #ifdef STG_TIME
 289     lastPing = stgTime;
 290     currTime = stgTime;
 291     #else
 292     currTime = lastPing = time(NULL);
 293     #endif
 294
 295     while (currTime - lastPing < pinger->delay && pinger->nonstop)
 296         {
 297         #ifdef STG_TIME
 298         currTime = stgTime;
 299         #else
 300         currTime = time(NULL);
 301         #endif
 302         struct timespec ts = {0, 20000000};
 303         nanosleep(&ts, NULL);
 304         }
 305     }
 306
 307 pinger->isRunningSender = false;
 308
 309 return NULL;
 310 }
 311 //-----------------------------------------------------------------------------
 312 void * STG_PINGER::RunRecvPing(void * d)
 313 {
 314 sigset_t signalSet;
 315 sigfillset(&signalSet);
 316 pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 317
 318 STG_PINGER * pinger = static_cast<STG_PINGER *>(d);
 319
 320 pinger->isRunningRecver = true;
 321
 322 while (pinger->nonstop)
 323     {
 324     uint32_t ip = pinger->RecvPing();
 325
 326     if (ip)
 327         {
 328         std::multimap<uint32_t, time_t>::iterator treeIterUpper = pinger->pingIP.upper_bound(ip);
 329         std::multimap<uint32_t, time_t>::iterator treeIterLower = pinger->pingIP.lower_bound(ip);
 330         while (treeIterUpper != treeIterLower)
 331             {
 332             #ifdef STG_TIME
 333             treeIterLower->second = stgTime;
 334             #else
 335             treeIterLower->second = time(NULL);
 336             #endif
 337             ++treeIterLower;
 338             }
 339         }
 340
 341     }
 342 pinger->isRunningRecver = false;
 343 return NULL;
 344 }
 345 //-----------------------------------------------------------------------------