4545
4646#include <nuttx/clock.h>
4747#include <nuttx/net/icmp.h>
48+ #include <nuttx/net/ip.h>
4849
4950#include "netutils/icmp_ping.h"
5051
@@ -64,7 +65,6 @@ struct ping_priv_s
6465{
6566 struct sockaddr_in destaddr ;
6667 struct sockaddr_in fromaddr ;
67- struct icmp_hdr_s outhdr ;
6868 struct pollfd recvfd ;
6969 socklen_t addrlen ;
7070 clock_t kickoff ;
@@ -74,6 +74,7 @@ struct ping_priv_s
7474 long elapsed ;
7575 bool retry ;
7676 int sockfd ;
77+ struct icmp_hdr_s outhdr ;
7778};
7879
7980/****************************************************************************
@@ -91,6 +92,7 @@ static volatile bool g_exiting;
9192 * Private Functions
9293 ****************************************************************************/
9394
95+ #ifdef CONFIG_ENABLE_ALL_SIGNALS
9496/****************************************************************************
9597 * Name: sigexit
9698 ****************************************************************************/
@@ -99,6 +101,7 @@ static void sigexit(int signo)
99101{
100102 g_exiting = true;
101103}
104+ #endif
102105
103106/****************************************************************************
104107 * Name: ping_newid
@@ -179,6 +182,31 @@ static void icmp_callback(FAR struct ping_result_s *result,
179182 result -> info -> callback (result );
180183}
181184
185+ /****************************************************************************
186+ * Name: icmp_checksum
187+ ****************************************************************************/
188+
189+ static uint16_t icmp_checksum (FAR const void * buffer , size_t datalen )
190+ {
191+ FAR const uint16_t * wptr = (FAR const uint16_t * )buffer ;
192+ size_t words = (sizeof (struct icmp_hdr_s ) + datalen + 1 ) / 2 ;
193+ uint32_t sum = 0 ;
194+ size_t i ;
195+
196+ for (i = 0 ; i < words ; i ++ )
197+ {
198+ sum += * wptr ++ ;
199+ if (sum & 0x80000000 )
200+ {
201+ sum = (sum & 0XFFFF ) + (sum >> 16 );
202+ }
203+ }
204+
205+ sum = (sum >> 16 ) + (sum & 0XFFFF );
206+ sum += (sum >> 16 );
207+ return (uint16_t )(~sum );
208+ }
209+
182210/****************************************************************************
183211 * Public Functions
184212 ****************************************************************************/
@@ -194,12 +222,15 @@ void icmp_ping(FAR const struct ping_info_s *info)
194222 FAR struct icmp_hdr_s * inhdr ;
195223 FAR uint8_t * iobuffer ;
196224 FAR uint8_t * ptr ;
225+ int recvlen ;
197226 int ret ;
198227 int ch ;
199228 int i ;
200229
201230 g_exiting = false;
231+ #ifdef CONFIG_ENABLE_ALL_SIGNALS
202232 signal (SIGINT , sigexit );
233+ #endif
203234
204235 /* Initialize result structure */
205236
@@ -215,16 +246,17 @@ void icmp_ping(FAR const struct ping_info_s *info)
215246
216247 /* Allocate memory to hold private data and ping buffer */
217248
218- priv = malloc (sizeof (* priv ) + result .outsize );
249+ priv = malloc (sizeof (* priv ) + sizeof (struct ipv4_hdr_s ) + result .outsize );
250+
219251 if (priv == NULL )
220252 {
221253 icmp_callback (& result , ICMP_E_MEMORY , 0 );
222254 return ;
223255 }
224256
225- iobuffer = (FAR uint8_t * )(priv + 1 );
257+ iobuffer = (FAR uint8_t * )(& priv -> outhdr );
226258
227- priv -> sockfd = socket (AF_INET , SOCK_DGRAM , IPPROTO_ICMP );
259+ priv -> sockfd = socket (AF_INET , SOCK_RAW , IPPROTO_ICMP );
228260 if (priv -> sockfd < 0 )
229261 {
230262 icmp_callback (& result , ICMP_E_SOCKET , errno );
@@ -253,11 +285,6 @@ void icmp_ping(FAR const struct ping_info_s *info)
253285 priv -> destaddr .sin_port = 0 ;
254286 priv -> destaddr .sin_addr .s_addr = result .dest .s_addr ;
255287
256- memset (& priv -> outhdr , 0 , sizeof (struct icmp_hdr_s ));
257- priv -> outhdr .type = ICMP_ECHO_REQUEST ;
258- priv -> outhdr .id = htons (result .id );
259- priv -> outhdr .seqno = htons (result .seqno );
260-
261288 icmp_callback (& result , ICMP_I_BEGIN , 0 );
262289
263290 while (result .nrequests < info -> count )
@@ -267,9 +294,10 @@ void icmp_ping(FAR const struct ping_info_s *info)
267294 break ;
268295 }
269296
270- /* Copy the ICMP header into the I/O buffer */
271-
272- memcpy (iobuffer , & priv -> outhdr , sizeof (struct icmp_hdr_s ));
297+ memset (& priv -> outhdr , 0 , sizeof (struct icmp_hdr_s ));
298+ priv -> outhdr .type = ICMP_ECHO_REQUEST ;
299+ priv -> outhdr .id = htons (result .id );
300+ priv -> outhdr .seqno = htons (result .seqno );
273301
274302 /* Add some easily verifiable payload data */
275303
@@ -285,6 +313,10 @@ void icmp_ping(FAR const struct ping_info_s *info)
285313 }
286314 }
287315
316+ /* Calculate checksum after data padding */
317+
318+ priv -> outhdr .icmpchksum = icmp_checksum (iobuffer , info -> datalen );
319+
288320 priv -> start = clock ();
289321 result .nrequests ++ ;
290322 priv -> nsent = sendto (priv -> sockfd , iobuffer , result .outsize , 0 ,
@@ -330,9 +362,10 @@ void icmp_ping(FAR const struct ping_info_s *info)
330362
331363 /* Get the ICMP response (ignoring the sender) */
332364
365+ recvlen = sizeof (struct ipv4_hdr_s ) + result .outsize ;
333366 priv -> addrlen = sizeof (struct sockaddr_in );
334367 priv -> nrecvd = recvfrom (priv -> sockfd , iobuffer ,
335- result . outsize , 0 ,
368+ recvlen , 0 ,
336369 (FAR struct sockaddr * )& priv -> fromaddr ,
337370 & priv -> addrlen );
338371 if (priv -> nrecvd < 0 )
@@ -346,8 +379,11 @@ void icmp_ping(FAR const struct ping_info_s *info)
346379 goto wait ;
347380 }
348381
382+ /* Skip IP header, IP header length including options */
383+
349384 priv -> elapsed = TICK2USEC (clock () - priv -> start );
350- inhdr = (FAR struct icmp_hdr_s * )iobuffer ;
385+ inhdr = (FAR struct icmp_hdr_s * )
386+ (iobuffer + sizeof (struct ipv4_hdr_s ));
351387
352388 if (inhdr -> type == ICMP_ECHO_REPLY )
353389 {
@@ -379,16 +415,17 @@ void icmp_ping(FAR const struct ping_info_s *info)
379415
380416 /* Verify the payload data */
381417
382- if (priv -> nrecvd != result . outsize )
418+ if (priv -> nrecvd != recvlen )
383419 {
384420 icmp_callback (& result , ICMP_W_RECVBIG , priv -> nrecvd );
385421 verified = false;
386422 }
387423 else
388424 {
389- ptr = & iobuffer [sizeof (struct icmp_hdr_s )];
390- ch = 0x20 ;
425+ /* Data start offset: IP header + ICMP header */
391426
427+ ptr = (FAR uint8_t * )(inhdr + 1 );
428+ ch = 0x20 ;
392429 for (i = 0 ; i < info -> datalen ; i ++ , ptr ++ )
393430 {
394431 if (* ptr != ch )
0 commit comments