]> git.stg.codes - stg.git/blob - libs/pinger/pinger.cpp
042edcba20d51872e5cbefc3468007d69e8936ee
[stg.git] / 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 //-----------------------------------------------------------------------------