1 #include "stdlib.h"
2 #include "lwip/opt.h"
3
4 #include "lwip/icmp.h"
5 #include "lwip/netif.h"
6 #include "lwip/sys.h"
7 #include "lwip/netdb.h"
8 #include "lwip/timeouts.h"
9 #include "lwip/inet_chksum.h"
10 #include "lwip/prot/ip.h"
11 #include "lwip/prot/ip4.h"
12 #include "lwip/sockets.h"
13
14 #include "vfsdev/wifi_dev.h"
15 #include "ulog/ulog.h"
16
17 #define PRINT_TAG "net_deamon"
18
19 #ifndef POLL_NET_STATUS_PERIOD_MS
20 #define POLL_NET_STATUS_PERIOD_MS 3000
21 #endif
22
23 #ifndef POLL_NET_STATUS_PERIOD_FAST_MS
24 #define POLL_NET_STATUS_PERIOD_FAST_MS 3000
25 #endif
26
27 #ifndef POLL_PING_RCV_TIMEO
28 #define POLL_PING_RCV_TIMEO 3000
29 #endif
30
31 #ifndef POLL_PING_COUNT
32 #define POLL_PING_COUNT 5
33 #endif
34
35 /** ping identifier - must fit on a u16_t */
36 #ifndef PING_ID
37 #define PING_ID 0xAFAF
38 #endif
39
40 /** ping delay - in milliseconds */
41 #ifndef POLL_PING_DELAY
42 #define POLL_PING_DELAY 1000
43 #endif
44
45 #define NET_DEAMON_TASK_STACK_SIZE 4096
46
47 static volatile int ping_seq_num = 0;
48 static volatile int daemon_started = 0;
49 static volatile int daemon_exit = 0;
50 static volatile u32_t ping_time = 0;
51 static volatile u32_t elapse_time = 0;
52 static volatile u32_t ping_success_count = 0;
53 static volatile u32_t ping_fail_count = 0;
54 static volatile float avg_elapse_time_ms = 0.0;
55
56 void get_gw_addr(ip4_addr_t *gw_addr);
57 /** Prepare a echo ICMP request */
58 static void
ping_prepare_echo(struct icmp_echo_hdr * iecho,u16_t len)59 ping_prepare_echo( struct icmp_echo_hdr *iecho, u16_t len)
60 {
61 size_t i;
62 size_t data_len = len - sizeof(struct icmp_echo_hdr);
63
64 ICMPH_TYPE_SET(iecho, ICMP_ECHO);
65 ICMPH_CODE_SET(iecho, 0);
66 iecho->chksum = 0;
67 iecho->id = PING_ID;
68 iecho->seqno = lwip_htons(++ping_seq_num);
69
70 /* fill the additional data buffer with some data */
71 for(i = 0; i < data_len; i++) {
72 ((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = (char)i;
73 }
74
75 iecho->chksum = inet_chksum(iecho, len);
76 }
77
78 /* Ping using the socket ip */
79 static err_t
ping_send(int s,const ip_addr_t * addr)80 ping_send(int s, const ip_addr_t *addr)
81 {
82 int err;
83 struct icmp_echo_hdr *iecho;
84 struct sockaddr_storage to;
85 size_t ping_size = sizeof(struct icmp_echo_hdr) + 5;
86 LWIP_ASSERT("ping_size is too big", ping_size <= 0xffff);
87
88 iecho = (struct icmp_echo_hdr *)mem_malloc((mem_size_t)ping_size);
89 if (!iecho) {
90 return ERR_MEM;
91 }
92
93 ping_prepare_echo(iecho, (u16_t)ping_size);
94
95 if(IP_IS_V4(addr)) {
96 struct sockaddr_in *to4 = (struct sockaddr_in*)&to;
97 to4->sin_len = sizeof(to4);
98 to4->sin_family = AF_INET;
99 inet_addr_from_ipaddr(&to4->sin_addr, ip_2_ip4(addr));
100 }
101
102 err = lwip_sendto(s, iecho, ping_size, 0, (struct sockaddr*)&to, sizeof(to));
103 ping_time = sys_now();
104 mem_free(iecho);
105 return (err ? ERR_OK : ERR_VAL);
106 }
107
108 static int
ping_recv(int s)109 ping_recv(int s)
110 {
111 char buf[64];
112 int len;
113 struct sockaddr_storage from;
114 int fromlen = sizeof(from);
115
116 while((len = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&from, (socklen_t*)&fromlen)) > 0) {
117 if (len >= (int)(sizeof(struct ip_hdr)+sizeof(struct icmp_echo_hdr))) {
118 ip_addr_t fromaddr;
119 memset(&fromaddr, 0, sizeof(fromaddr));
120
121 if(from.ss_family == AF_INET) {
122 struct sockaddr_in *from4 = (struct sockaddr_in*)&from;
123 inet_addr_to_ipaddr(ip_2_ip4(&fromaddr), &from4->sin_addr);
124 IP_SET_TYPE_VAL(fromaddr, IPADDR_TYPE_V4);
125 }
126
127 if (IP_IS_V4_VAL(fromaddr)) {
128 struct ip_hdr *iphdr;
129 struct icmp_echo_hdr *iecho;
130
131 iphdr = (struct ip_hdr *)buf;
132 iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4));
133 if ((iecho->id == PING_ID) && (iecho->seqno == lwip_htons(ping_seq_num))) {
134 /* do some ping result processing */
135 elapse_time = aos_now_ms() - ping_time;
136 return 0;
137 } else {
138 LWIP_DEBUGF( PING_DEBUG, ("ping: drop\n"));
139 }
140 }
141 }
142 fromlen = sizeof(from);
143 }
144
145 if (len == 0) {
146 LWIP_DEBUGF( PING_DEBUG, ("ping: recv - timeout\n"));
147 }
148
149 return -1;
150 }
151
net_daemon_thread(void * arg)152 void net_daemon_thread(void *arg)
153 {
154 int s;
155 int ret;
156 int count;
157 int success_count;
158 ip4_addr_t *target = (ip4_addr_t *)arg;
159 ip4_addr_t *now_addr = NULL;
160 ip_addr_t ping_target;
161 struct timeval timeout;
162 int ping_recv_timeout = POLL_PING_RCV_TIMEO;
163
164 if (!target) {
165 LWIP_DEBUGF( PING_DEBUG, ("Invalid arg!"));
166 return;
167 }
168
169 now_addr = (ip4_addr_t *)malloc(sizeof(ip4_addr_t));
170 if (!now_addr) {
171 return;
172 }
173
174 timeout.tv_sec = ping_recv_timeout/1000;
175 timeout.tv_usec = (ping_recv_timeout%1000)*1000;
176
177 ping_target = *target;
178
179 free(target);
180 target = NULL;
181
182 if (daemon_started) {
183 LWIP_DEBUGF( PING_DEBUG, ("Already started!"));
184 free(now_addr);
185 return;
186 }
187
188 daemon_started = 1;
189
190 s = lwip_socket(AF_INET, SOCK_RAW, IP_PROTO_ICMP);
191 if (s == -1) {
192 LWIP_DEBUGF( PING_DEBUG, ("ping: create socket failed\n"));
193 goto exit;
194 }
195
196 ret = lwip_setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
197 LWIP_ASSERT("setting receive timeout failed", ret == 0);
198 LWIP_UNUSED_ARG(ret);
199
200 do {
201 count = POLL_PING_COUNT;
202 success_count = 0;
203 ping_seq_num = 0;
204
205 get_gw_addr(now_addr);
206 ping_target = *now_addr;
207
208 while (1) {
209 if (daemon_exit) {
210 goto exit;
211 }
212
213 if(count <= 0) {
214 LOGW(PRINT_TAG, "ping: send finished\n");
215 break;
216 }
217
218 if(count > 0) {
219 count --;
220 }
221
222 if (ping_send(s, &ping_target) == ERR_OK) {
223 if (ping_recv(s) == ERR_OK) {
224 success_count++;
225 break;
226 }
227 }
228
229 sys_msleep(POLL_PING_DELAY);
230 }
231
232 if (success_count == 0) {
233 // reboot netif and exit
234 LOGW(PRINT_TAG, "==> Attention! Fail to ping router! <===\n");
235 ping_fail_count++;
236 sys_msleep(POLL_NET_STATUS_PERIOD_FAST_MS);
237 } else {
238 LOGI(PRINT_TAG, "==> ping router success! %"U32_F" ms <===\n", elapse_time);
239 avg_elapse_time_ms = (avg_elapse_time_ms * ping_success_count + elapse_time) / (ping_success_count + 1);
240 ping_success_count++;
241
242 sys_msleep(POLL_NET_STATUS_PERIOD_MS);
243 }
244 } while (1);
245
246 exit:
247 if (s != -1) {
248 lwip_close(s);
249 }
250 daemon_started = 0;
251 daemon_exit = 0;
252 aos_task_exit(0);
253 free(now_addr);
254 }
255
start_net_deamon(void)256 void start_net_deamon(void)
257 {
258 #if 0
259 int ret;
260 wifi_ip_stat_t ip_stat;
261 ip4_addr_t *gw_addr = NULL;
262
263 if (daemon_started) {
264 LWIP_DEBUGF( PING_DEBUG, ("Netde Already started!\n"));
265 return;
266 }
267
268 gw_addr = (ip4_addr_t *)malloc(sizeof(ip4_addr_t));
269 if (!gw_addr)
270 return;
271
272 memset(&ip_stat, 0, sizeof(ip_stat));
273 ret = wifi_get_ip_stat(NULL, &ip_stat, STATION);
274 if (ret != 0) {
275 LWIP_DEBUGF( PING_DEBUG, ("%s get ip fail\r\n", __func__));
276 free(gw_addr);
277 return;
278 }
279
280 gw_addr->addr = ipaddr_addr(ip_stat.gate);
281 if (IPADDR_NONE == gw_addr->addr) {
282 LWIP_DEBUGF( PING_DEBUG, ("Convert ipaddr addr failed!\n"));
283 free(gw_addr);
284 return;
285 }
286
287 if ( 0 != aos_task_new("net_deamon", net_daemon_thread,
288 gw_addr, NET_DEAMON_TASK_STACK_SIZE)) {
289 free(gw_addr);
290 }
291 #endif
292 }
293
get_gw_addr(ip4_addr_t * gw_addr)294 void get_gw_addr(ip4_addr_t *gw_addr)
295 {
296 int ret = 0;
297 wifi_ip_stat_t ip_stat = {0};
298 //ret = wifi_get_ip_stat(NULL, &ip_stat, STATION);
299 if (ret != 0) {
300 LWIP_DEBUGF( PING_DEBUG, ("%s get ip fail\r\n", __func__));
301 return;
302 }
303 gw_addr->addr = ipaddr_addr(ip_stat.gate);
304 if (IPADDR_NONE == gw_addr->addr) {
305 LWIP_DEBUGF( PING_DEBUG, ("Convert ipaddr addr failed!\n"));
306 return;
307 }
308 return;
309 }
stop_net_deamon(void)310 void stop_net_deamon(void)
311 {
312 daemon_exit = 1;
313 }
314
get_poll_ping_success_count(uint32_t * value,int clear)315 u32_t get_poll_ping_success_count(uint32_t *value, int clear)
316 {
317 if (clear){
318 ping_success_count = 0;
319 }
320 *value = ping_success_count;
321 return ping_success_count;
322 }
323
get_poll_ping_fail_count(uint32_t * value,int clear)324 u32_t get_poll_ping_fail_count(uint32_t *value, int clear)
325 {
326 if (clear){
327 ping_fail_count = 0;
328 }
329 *value = ping_fail_count;
330 return 0;
331 }
332
get_poll_ping_avg_elapse_ms(float * value,int clear)333 void get_poll_ping_avg_elapse_ms(float *value, int clear)
334 {
335 if (clear){
336 avg_elapse_time_ms = 0.0;
337 }
338 //LOGI(PRINT_TAG, "get avg_ping = %lf\n",avg_elapse_time_ms);
339 *value = avg_elapse_time_ms;
340 }
341
get_poll_last_ping_elapse_ms(uint32_t * value)342 void get_poll_last_ping_elapse_ms(uint32_t *value)
343 {
344 *value = elapse_time;
345 }
346
347