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     : m_delay(d),
  30       m_nonstop(false),
  31       m_isRunningRecver(false),
  32       m_isRunningSender(false),
  33       m_sendSocket(-1),
  34       m_recvSocket(-1),
  35       m_pid(0)
  36 {
  37 pthread_mutex_init(&m_mutex, NULL);
  38 memset(&m_pmSend, 0, sizeof(m_pmSend));
  39 }
  40 //-----------------------------------------------------------------------------
  41 STG_PINGER::~STG_PINGER()
  42 {
  43 pthread_mutex_destroy(&m_mutex);
  44 }
  45 //-----------------------------------------------------------------------------
  46 int STG_PINGER::Start()
  47 {
  48 struct protoent *proto = NULL;
  49 proto = getprotobyname("ICMP");
  50 m_sendSocket = socket(PF_INET, SOCK_RAW, proto->p_proto);
  51 m_recvSocket = socket(PF_INET, SOCK_RAW, proto->p_proto);
  52 m_nonstop = true;
  53 m_pid = static_cast<uint32_t>(getpid()) % 65535;
  54 if (m_sendSocket < 0 || m_recvSocket < 0)
  55     {
  56     m_errorStr = "Cannot create socket.";
  57     return -1;
  58     }
  59
  60 if (pthread_create(&m_sendThread, NULL, RunSendPing, this))
  61     {
  62     m_errorStr = "Cannot create send thread.";
  63     return -1;
  64     }
  65
  66 if (pthread_create(&m_recvThread, NULL, RunRecvPing, this))
  67     {
  68     m_errorStr = "Cannot create recv thread.";
  69     return -1;
  70     }
  71
  72 return 0;
  73 }
  74 //-----------------------------------------------------------------------------
  75 int STG_PINGER::Stop()
  76 {
  77 close(m_recvSocket);
  78 m_nonstop = false;
  79 if (m_isRunningRecver)
  80     {
  81     //5 seconds to thread stops itself
  82     for (size_t i = 0; i < 25; i++)
  83         {
  84         if (i % 5 == 0)
  85             SendPing(0x0100007f);//127.0.0.1
  86
  87         if (!m_isRunningRecver)
  88             break;
  89
  90         struct timespec ts = {0, 200000000};
  91         nanosleep(&ts, NULL);
  92         }
  93     }
  94
  95 if (m_isRunningSender)
  96     {
  97     //5 seconds to thread stops itself
  98     for (size_t i = 0; i < 25; i++)
  99         {
 100         if (!m_isRunningSender)
 101             break;
 102
 103         struct timespec ts = {0, 200000000};
 104         nanosleep(&ts, NULL);
 105         }
 106     }
 107
 108 close(m_sendSocket);
 109
 110 if (m_isRunningSender || m_isRunningRecver)
 111     return -1;
 112
 113 return 0;
 114 }
 115 //-----------------------------------------------------------------------------
 116 void STG_PINGER::AddIP(uint32_t ip)
 117 {
 118 STG_LOCKER lock(&m_mutex);
 119 m_ipToAdd.push_back(ip);
 120 }
 121 //-----------------------------------------------------------------------------
 122 void STG_PINGER::DelIP(uint32_t ip)
 123 {
 124 STG_LOCKER lock(&m_mutex);
 125 m_ipToDel.push_back(ip);
 126 }
 127 //-----------------------------------------------------------------------------
 128 void STG_PINGER::RealAddIP()
 129 {
 130 STG_LOCKER lock(&m_mutex);
 131
 132 auto iter = m_ipToAdd.begin();
 133 while (iter != m_ipToAdd.end())
 134     {
 135     m_pingIP.insert(std::make_pair(*iter, 0));
 136     ++iter;
 137     }
 138 m_ipToAdd.erase(m_ipToAdd.begin(), m_ipToAdd.end());
 139 }
 140 //-----------------------------------------------------------------------------
 141 void STG_PINGER::RealDelIP()
 142 {
 143 STG_LOCKER lock(&m_mutex);
 144
 145 auto iter = m_ipToDel.begin();
 146 while (iter != m_ipToDel.end())
 147     {
 148     auto treeIter = m_pingIP.find(*iter);
 149     if (treeIter != m_pingIP.end())
 150         m_pingIP.erase(treeIter);
 151
 152     ++iter;
 153     }
 154 m_ipToDel.erase(m_ipToDel.begin(), m_ipToDel.end());
 155 }
 156 //-----------------------------------------------------------------------------
 157 void STG_PINGER::PrintAllIP()
 158 {
 159 STG_LOCKER lock(&m_mutex);
 160 auto iter = m_pingIP.begin();
 161 while (iter != m_pingIP.end())
 162     {
 163     uint32_t ip = iter->first;
 164     time_t t = iter->second;
 165     std::string s = std::to_string(t);
 166     printf("ip = %s, time = %9s\n", inet_ntostring(ip).c_str(), s.c_str());
 167     ++iter;
 168     }
 169
 170 }
 171 //-----------------------------------------------------------------------------
 172 int STG_PINGER::GetIPTime(uint32_t ip, time_t * t) const
 173 {
 174 STG_LOCKER lock(&m_mutex);
 175
 176 auto treeIter = m_pingIP.find(ip);
 177 if (treeIter == m_pingIP.end())
 178     return -1;
 179
 180 *t = treeIter->second;
 181 return 0;
 182 }
 183 //-----------------------------------------------------------------------------
 184 uint16_t STG_PINGER::PingCheckSum(void * data, int len)
 185 {
 186 uint16_t * buf = static_cast<uint16_t *>(data);
 187 uint32_t sum = 0;
 188 uint32_t result;
 189
 190 for ( sum = 0; len > 1; len -= 2 )
 191     sum += *buf++;
 192
 193 if ( len == 1 )
 194     sum += *reinterpret_cast<uint8_t*>(buf);
 195
 196 sum = (sum >> 16) + (sum & 0xFFFF);
 197 sum += (sum >> 16);
 198 result = ~sum;
 199 return static_cast<uint16_t>(result);
 200 }
 201 //-----------------------------------------------------------------------------
 202 int STG_PINGER::SendPing(uint32_t ip)
 203 {
 204 struct sockaddr_in addr;
 205 memset(&addr, 0, sizeof(addr));
 206 addr.sin_family = AF_INET;
 207 addr.sin_port = 0;
 208 addr.sin_addr.s_addr = ip;
 209
 210 memset(&m_pmSend, 0, sizeof(m_pmSend));
 211 m_pmSend.hdr.type = ICMP_ECHO;
 212 m_pmSend.hdr.un.echo.id = static_cast<uint16_t>(m_pid);
 213 memcpy(m_pmSend.msg, &ip, sizeof(ip));
 214
 215 m_pmSend.hdr.checksum = PingCheckSum(&m_pmSend, sizeof(m_pmSend));
 216
 217 if (sendto(m_sendSocket, &m_pmSend, sizeof(m_pmSend), 0, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) <= 0 )
 218     {
 219     m_errorStr = "Send ping error: " + std::string(strerror(errno));
 220     return -1;
 221     }
 222
 223
 224 return 0;
 225 }
 226 //-----------------------------------------------------------------------------
 227 uint32_t STG_PINGER::RecvPing()
 228 {
 229 struct sockaddr_in addr;
 230 uint32_t ipAddr = 0;
 231
 232 char buf[128];
 233 memset(buf, 0, sizeof(buf));
 234 socklen_t len = sizeof(addr);
 235
 236 if (recvfrom(m_recvSocket, &buf, sizeof(buf), 0, reinterpret_cast<struct sockaddr*>(&addr), &len))
 237     {
 238     struct IP_HDR * ip = static_cast<struct IP_HDR *>(static_cast<void *>(buf));
 239     struct ICMP_HDR *icmp = static_cast<struct ICMP_HDR *>(static_cast<void *>(buf + ip->ihl * 4));
 240
 241     if (icmp->un.echo.id != m_pid)
 242         return 0;
 243
 244     ipAddr = *static_cast<uint32_t*>(static_cast<void *>(buf + sizeof(ICMP_HDR) + ip->ihl * 4));
 245     }
 246
 247 return ipAddr;
 248 }
 249 //-----------------------------------------------------------------------------
 250 void * STG_PINGER::RunSendPing(void * d)
 251 {
 252 sigset_t signalSet;
 253 sigfillset(&signalSet);
 254 pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 255
 256 auto* pinger = static_cast<STG_PINGER *>(d);
 257
 258 pinger->m_isRunningSender = true;
 259 time_t lastPing = 0;
 260 while (pinger->m_nonstop)
 261     {
 262     pinger->RealAddIP();
 263     pinger->RealDelIP();
 264
 265     std::multimap<uint32_t, time_t>::iterator iter;
 266     iter = pinger->m_pingIP.begin();
 267     while (iter != pinger->m_pingIP.end())
 268         {
 269         pinger->SendPing(iter->first);
 270         ++iter;
 271         }
 272
 273     time_t currTime;
 274
 275     #ifdef STG_TIME
 276     lastPing = stgTime;
 277     currTime = stgTime;
 278     #else
 279     currTime = lastPing = time(NULL);
 280     #endif
 281
 282     while (currTime - lastPing < pinger->m_delay && pinger->m_nonstop)
 283         {
 284         #ifdef STG_TIME
 285         currTime = stgTime;
 286         #else
 287         currTime = time(NULL);
 288         #endif
 289         struct timespec ts = {0, 20000000};
 290         nanosleep(&ts, NULL);
 291         }
 292     }
 293
 294 pinger->m_isRunningSender = false;
 295
 296 return NULL;
 297 }
 298 //-----------------------------------------------------------------------------
 299 void * STG_PINGER::RunRecvPing(void * d)
 300 {
 301 sigset_t signalSet;
 302 sigfillset(&signalSet);
 303 pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 304
 305 auto* pinger = static_cast<STG_PINGER *>(d);
 306
 307 pinger->m_isRunningRecver = true;
 308
 309 while (pinger->m_nonstop)
 310     {
 311     uint32_t ip = pinger->RecvPing();
 312
 313     if (ip)
 314         {
 315         auto treeIterUpper = pinger->m_pingIP.upper_bound(ip);
 316         auto treeIterLower = pinger->m_pingIP.lower_bound(ip);
 317         while (treeIterUpper != treeIterLower)
 318             {
 319             #ifdef STG_TIME
 320             treeIterLower->second = stgTime;
 321             #else
 322             treeIterLower->second = time(NULL);
 323             #endif
 324             ++treeIterLower;
 325             }
 326         }
 327
 328     }
 329 pinger->m_isRunningRecver = false;
 330 return NULL;
 331 }
 332 //-----------------------------------------------------------------------------