in netutils/ping/icmp_ping.c [190:437]
void icmp_ping(FAR const struct ping_info_s *info)
{
struct ping_result_s result;
FAR struct ping_priv_s *priv;
FAR struct icmp_hdr_s *inhdr;
FAR uint8_t *iobuffer;
FAR uint8_t *ptr;
int ret;
int ch;
int i;
g_exiting = false;
signal(SIGINT, sigexit);
/* Initialize result structure */
memset(&result, 0, sizeof(result));
result.info = info;
result.id = ping_newid();
result.outsize = ICMP_IOBUFFER_SIZE(info->datalen);
if (ping_gethostip(info->hostname, &result.dest) < 0)
{
icmp_callback(&result, ICMP_E_HOSTIP, 0);
return;
}
/* Allocate memory to hold private data and ping buffer */
priv = malloc(sizeof(*priv) + result.outsize);
if (priv == NULL)
{
icmp_callback(&result, ICMP_E_MEMORY, 0);
return;
}
iobuffer = (FAR uint8_t *)(priv + 1);
priv->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_ICMP);
if (priv->sockfd < 0)
{
icmp_callback(&result, ICMP_E_SOCKET, errno);
free(priv);
return;
}
priv->kickoff = clock();
memset(&priv->destaddr, 0, sizeof(struct sockaddr_in));
priv->destaddr.sin_family = AF_INET;
priv->destaddr.sin_port = 0;
priv->destaddr.sin_addr.s_addr = result.dest.s_addr;
memset(&priv->outhdr, 0, sizeof(struct icmp_hdr_s));
priv->outhdr.type = ICMP_ECHO_REQUEST;
priv->outhdr.id = htons(result.id);
priv->outhdr.seqno = htons(result.seqno);
icmp_callback(&result, ICMP_I_BEGIN, 0);
while (result.nrequests < info->count)
{
if (g_exiting)
{
break;
}
/* Copy the ICMP header into the I/O buffer */
memcpy(iobuffer, &priv->outhdr, sizeof(struct icmp_hdr_s));
/* Add some easily verifiable payload data */
ptr = &iobuffer[sizeof(struct icmp_hdr_s)];
ch = 0x20;
for (i = 0; i < info->datalen; i++)
{
*ptr++ = ch;
if (++ch > 0x7e)
{
ch = 0x20;
}
}
priv->start = clock();
result.nrequests++;
priv->nsent = sendto(priv->sockfd, iobuffer, result.outsize, 0,
(FAR struct sockaddr *)&priv->destaddr,
sizeof(struct sockaddr_in));
if (priv->nsent < 0)
{
icmp_callback(&result, ICMP_E_SENDTO, errno);
goto wait;
}
else if (priv->nsent != result.outsize)
{
icmp_callback(&result, ICMP_E_SENDSMALL, priv->nsent);
goto wait;
}
priv->elapsed = 0;
do
{
priv->retry = false;
priv->recvfd.fd = priv->sockfd;
priv->recvfd.events = POLLIN;
priv->recvfd.revents = 0;
ret = poll(&priv->recvfd, 1,
info->timeout - priv->elapsed / USEC_PER_MSEC);
if (ret < 0)
{
icmp_callback(&result, ICMP_E_POLL, errno);
goto wait;
}
else if (ret == 0)
{
icmp_callback(&result, ICMP_W_TIMEOUT, info->timeout);
goto wait;
}
if (priv->recvfd.revents & (POLLHUP | POLLERR))
{
icmp_callback(&result, ICMP_E_POLL, ENETDOWN);
goto wait;
}
/* Get the ICMP response (ignoring the sender) */
priv->addrlen = sizeof(struct sockaddr_in);
priv->nrecvd = recvfrom(priv->sockfd, iobuffer,
result.outsize, 0,
(FAR struct sockaddr *)&priv->fromaddr,
&priv->addrlen);
if (priv->nrecvd < 0)
{
icmp_callback(&result, ICMP_E_RECVFROM, errno);
goto wait;
}
else if (priv->nrecvd < sizeof(struct icmp_hdr_s))
{
icmp_callback(&result, ICMP_E_RECVSMALL, priv->nrecvd);
goto wait;
}
priv->elapsed = TICK2USEC(clock() - priv->start);
inhdr = (FAR struct icmp_hdr_s *)iobuffer;
if (inhdr->type == ICMP_ECHO_REPLY)
{
#ifndef CONFIG_SIM_NETUSRSOCK
if (ntohs(inhdr->id) != result.id)
{
icmp_callback(&result, ICMP_W_IDDIFF, ntohs(inhdr->id));
priv->retry = true;
}
else
#endif
if (ntohs(inhdr->seqno) > result.seqno)
{
icmp_callback(&result, ICMP_W_SEQNOBIG,
ntohs(inhdr->seqno));
priv->retry = true;
}
else if (ntohs(inhdr->seqno) < result.seqno)
{
icmp_callback(&result, ICMP_W_SEQNOSMALL,
ntohs(inhdr->seqno));
priv->retry = true;
}
else
{
bool verified = true;
icmp_callback(&result, ICMP_I_ROUNDTRIP, priv->elapsed);
/* Verify the payload data */
if (priv->nrecvd != result.outsize)
{
icmp_callback(&result, ICMP_W_RECVBIG, priv->nrecvd);
verified = false;
}
else
{
ptr = &iobuffer[sizeof(struct icmp_hdr_s)];
ch = 0x20;
for (i = 0; i < info->datalen; i++, ptr++)
{
if (*ptr != ch)
{
icmp_callback(&result, ICMP_W_DATADIFF, 0);
verified = false;
break;
}
if (++ch > 0x7e)
{
ch = 0x20;
}
}
}
/* Only count the number of good replies */
if (verified)
{
result.nreplies++;
}
}
}
else
{
icmp_callback(&result, ICMP_W_TYPE, inhdr->type);
}
}
while (priv->retry && info->delay > priv->elapsed / USEC_PER_MSEC &&
info->timeout > priv->elapsed / USEC_PER_MSEC);
/* Wait if necessary to preserved the requested ping rate */
wait:
priv->elapsed = TICK2MSEC(clock() - priv->start);
if (priv->elapsed < info->delay)
{
struct timespec rqt;
unsigned int remaining;
unsigned int sec;
unsigned int frac; /* In deciseconds */
remaining = info->delay - priv->elapsed;
sec = remaining / MSEC_PER_SEC;
frac = remaining - MSEC_PER_SEC * sec;
rqt.tv_sec = sec;
rqt.tv_nsec = frac * NSEC_PER_MSEC;
nanosleep(&rqt, NULL);
}
priv->outhdr.seqno = htons(++result.seqno);
}
icmp_callback(&result, ICMP_I_FINISH, TICK2USEC(clock() - priv->kickoff));
close(priv->sockfd);
free(priv);
}