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