libs/pinger/pinger.cpp

   1 #include "stg/pinger.h"
   2
   3 #include "stg/common.h"
   4
   5 #include <chrono>
   6 #include <cstdlib>
   7 #include <csignal>
   8 #include <cstring>
   9 #include <cerrno>
  10 #include <cmath>
  11 #include <cstdio>
  12
  13 #include <netdb.h>
  14 #include <sys/types.h>
  15 #include <sys/socket.h>
  16 #include <netinet/in.h>
  17 #include <arpa/inet.h>
  18 #include <sys/time.h>
  19 #include <unistd.h>
  20 #include <fcntl.h>
  21
  22 #ifdef STG_TIME
  23 extern volatile time_t stgTime;
  24 #endif
  25
  26 //-----------------------------------------------------------------------------
  27 STG_PINGER::STG_PINGER(unsigned d)
  28     : m_delay(d),
  29       m_isRunningRecver(false),
  30       m_isRunningSender(false),
  31       m_sendSocket(-1),
  32       m_recvSocket(-1),
  33       m_pid(0)
  34 {
  35     memset(&m_pmSend, 0, sizeof(m_pmSend));
  36 }
  37 //-----------------------------------------------------------------------------
  38 bool STG_PINGER::Start()
  39 {
  40     auto* proto = getprotobyname("ICMP");
  41     m_sendSocket = socket(PF_INET, SOCK_RAW, proto->p_proto);
  42     m_recvSocket = socket(PF_INET, SOCK_RAW, proto->p_proto);
  43     m_pid = static_cast<uint32_t>(getpid()) % 65535;
  44     if (m_sendSocket < 0 || m_recvSocket < 0)
  45     {
  46         m_errorStr = "Cannot create socket.";
  47         return false;
  48     }
  49
  50     m_sendThread = std::jthread([this](auto token){ RunSendPing(std::move(token)); });
  51     m_recvThread = std::jthread([this](auto token){ RunRecvPing(std::move(token)); });
  52
  53     return true;
  54 }
  55 //-----------------------------------------------------------------------------
  56 bool STG_PINGER::Stop()
  57 {
  58     close(m_recvSocket);
  59     m_sendThread.request_stop();
  60     if (m_isRunningRecver)
  61     {
  62         //5 seconds to thread stops itself
  63         for (size_t i = 0; i < 25; i++)
  64         {
  65             if (i % 5 == 0)
  66                 SendPing(0x0100007f);//127.0.0.1
  67
  68             if (!m_isRunningRecver)
  69                 break;
  70
  71             std::this_thread::sleep_for(std::chrono::milliseconds(200));
  72         }
  73     }
  74
  75     if (m_isRunningSender)
  76     {
  77         //5 seconds to thread stops itself
  78         for (size_t i = 0; i < 25; i++)
  79         {
  80             if (!m_isRunningSender)
  81                 break;
  82
  83             std::this_thread::sleep_for(std::chrono::milliseconds(200));
  84         }
  85     }
  86
  87     close(m_sendSocket);
  88
  89     return !m_isRunningSender && !m_isRunningRecver;
  90 }
  91 //-----------------------------------------------------------------------------
  92 void STG_PINGER::AddIP(uint32_t ip)
  93 {
  94     std::lock_guard lock(m_mutex);
  95     m_pingIPs.insert(std::make_pair(ip, 0));
  96 }
  97 //-----------------------------------------------------------------------------
  98 void STG_PINGER::DelIP(uint32_t ip)
  99 {
 100     std::lock_guard lock(m_mutex);
 101     auto it = m_pingIPs.find(ip);
 102     if (it != m_pingIPs.end())
 103         m_pingIPs.erase(it);
 104 }
 105 //-----------------------------------------------------------------------------
 106 void STG_PINGER::PrintAllIP()
 107 {
 108     std::lock_guard lock(m_mutex);
 109     for (auto kv : m_pingIPs)
 110     {
 111         uint32_t ip = kv.first;
 112         time_t t = kv.second;
 113         std::string s = std::to_string(t);
 114         printf("ip = %s, time = %9s\n", inet_ntostring(ip).c_str(), s.c_str());
 115     }
 116 }
 117 //-----------------------------------------------------------------------------
 118 bool STG_PINGER::GetIPTime(uint32_t ip, time_t& t) const
 119 {
 120     std::lock_guard lock(m_mutex);
 121
 122     auto it = m_pingIPs.find(ip);
 123     if (it == m_pingIPs.end())
 124         return false;
 125
 126     t = it->second;
 127     return true;
 128 }
 129 //-----------------------------------------------------------------------------
 130 uint16_t STG_PINGER::PingCheckSum(const void* data, int len)
 131 {
 132     const auto* buf = static_cast<const uint16_t*>(data);
 133     uint32_t sum = 0;
 134
 135     for ( sum = 0; len > 1; len -= 2 )
 136         sum += *buf++;
 137
 138     if ( len == 1 )
 139         sum += *reinterpret_cast<const uint8_t*>(buf);
 140
 141     sum = (sum >> 16) + (sum & 0xFFFF);
 142     sum += (sum >> 16);
 143
 144     return ~sum;
 145 }
 146 //-----------------------------------------------------------------------------
 147 bool STG_PINGER::SendPing(uint32_t ip)
 148 {
 149     struct sockaddr_in addr;
 150     memset(&addr, 0, sizeof(addr));
 151     addr.sin_family = AF_INET;
 152     addr.sin_port = 0;
 153     addr.sin_addr.s_addr = ip;
 154
 155     memset(&m_pmSend, 0, sizeof(m_pmSend));
 156     m_pmSend.hdr.type = ICMP_ECHO;
 157     m_pmSend.hdr.un.echo.id = static_cast<uint16_t>(m_pid);
 158     memcpy(m_pmSend.msg, &ip, sizeof(ip));
 159
 160     m_pmSend.hdr.checksum = PingCheckSum(&m_pmSend, sizeof(m_pmSend));
 161
 162     if (sendto(m_sendSocket, &m_pmSend, sizeof(m_pmSend), 0, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) <= 0 )
 163     {
 164         m_errorStr = "Send ping error: " + std::string(strerror(errno));
 165         return false;
 166     }
 167
 168
 169     return true;
 170 }
 171 //-----------------------------------------------------------------------------
 172 uint32_t STG_PINGER::RecvPing()
 173 {
 174     struct sockaddr_in addr;
 175     uint32_t ipAddr = 0;
 176
 177     uint8_t buf[128];
 178     memset(buf, 0, sizeof(buf));
 179     socklen_t len = sizeof(addr);
 180
 181     if (recvfrom(m_recvSocket, &buf, sizeof(buf), 0, reinterpret_cast<struct sockaddr*>(&addr), &len))
 182     {
 183         auto* ip = static_cast<struct IP_HDR *>(static_cast<void *>(buf));
 184         auto* icmp = static_cast<struct ICMP_HDR *>(static_cast<void *>(buf + ip->ihl * 4));
 185
 186         if (icmp->un.echo.id != m_pid)
 187             return 0;
 188
 189         ipAddr = *static_cast<uint32_t*>(static_cast<void *>(buf + sizeof(ICMP_HDR) + ip->ihl * 4));
 190     }
 191
 192     return ipAddr;
 193 }
 194 //-----------------------------------------------------------------------------
 195 void STG_PINGER::RunSendPing(std::stop_token token)
 196 {
 197     sigset_t signalSet;
 198     sigfillset(&signalSet);
 199     pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 200
 201     m_isRunningSender = true;
 202     time_t lastPing = 0;
 203     while (!token.stop_requested())
 204     {
 205         PingIPs ips;
 206         {
 207             std::lock_guard lock(m_mutex);
 208             ips = m_pingIPs;
 209         }
 210
 211         for (const auto& kv : ips)
 212             SendPing(kv.first);
 213
 214         time_t currTime;
 215
 216         #ifdef STG_TIME
 217         lastPing = stgTime;
 218         currTime = stgTime;
 219         #else
 220         currTime = lastPing = time(NULL);
 221         #endif
 222
 223         while (currTime - lastPing < m_delay && !token.stop_requested())
 224         {
 225             #ifdef STG_TIME
 226             currTime = stgTime;
 227             #else
 228             currTime = time(NULL);
 229             #endif
 230             std::this_thread::sleep_for(std::chrono::milliseconds(200));
 231         }
 232     }
 233
 234     m_isRunningSender = false;
 235 }
 236 //-----------------------------------------------------------------------------
 237 void STG_PINGER::RunRecvPing(std::stop_token token)
 238 {
 239     sigset_t signalSet;
 240     sigfillset(&signalSet);
 241     pthread_sigmask(SIG_BLOCK, &signalSet, NULL);
 242
 243     m_isRunningRecver = true;
 244
 245     while (!token.stop_requested())
 246     {
 247         uint32_t ip = RecvPing();
 248
 249         if (ip)
 250         {
 251             std::lock_guard lock(m_mutex);
 252             auto treeIterUpper = m_pingIPs.upper_bound(ip);
 253             auto treeIterLower = m_pingIPs.lower_bound(ip);
 254             while (treeIterUpper != treeIterLower)
 255             {
 256                 #ifdef STG_TIME
 257                 treeIterLower->second = stgTime;
 258                 #else
 259                 treeIterLower->second = time(NULL);
 260                 #endif
 261                 ++treeIterLower;
 262             }
 263         }
 264
 265     }
 266     m_isRunningRecver = false;
 267 }
 268 //-----------------------------------------------------------------------------