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