You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
262 lines
7.9 KiB
262 lines
7.9 KiB
// based on https://github.com/pbecchi/ESP32_ping/blob/master/Ping.cpp
|
|
|
|
#include <string.h>
|
|
|
|
#include "ping.h"
|
|
|
|
#include "esp_log.h"
|
|
|
|
#include "lwip/inet_chksum.h"
|
|
#include "lwip/ip.h"
|
|
#include "lwip/ip4.h"
|
|
#include "lwip/err.h"
|
|
#include "lwip/icmp.h"
|
|
#include "lwip/sockets.h"
|
|
#include "lwip/sys.h"
|
|
#include "lwip/netdb.h"
|
|
#include "lwip/dns.h"
|
|
|
|
static const char *TAG = "ping";
|
|
|
|
typedef struct {
|
|
ping_opts_t config;
|
|
|
|
int sockfd;
|
|
uint16_t ping_seq_num; // sequence number for the next packet
|
|
uint16_t transmitted; // sent requests
|
|
uint16_t received; // received responses
|
|
uint16_t min_time_ms;
|
|
uint16_t max_time_ms;
|
|
uint16_t last_delay_ms;
|
|
} ping_session_t;
|
|
|
|
#define PING_ID 0xABCD
|
|
|
|
static void ping_prepare_echo(ping_session_t *session, struct icmp_echo_hdr *echohdr)
|
|
{
|
|
const size_t hdr_len = sizeof(struct icmp_echo_hdr);
|
|
const size_t payload_len = session->config.payload_size;
|
|
|
|
ICMPH_TYPE_SET(echohdr, ICMP_ECHO); // compatibility alias
|
|
ICMPH_CODE_SET(echohdr, 0);
|
|
echohdr->chksum = 0;
|
|
echohdr->id = PING_ID;
|
|
echohdr->seqno = htons(++session->ping_seq_num);
|
|
|
|
// the packet is longer than the header, it was malloc'd with extra space
|
|
// at the end for the payload
|
|
|
|
/* fill the rest of the buffer with dummy data */
|
|
for (size_t i = 0; i < payload_len; i++) {
|
|
((char *) echohdr)[hdr_len + i] = (char) (' ' + i);
|
|
}
|
|
|
|
echohdr->chksum = inet_chksum(echohdr, (u16_t) (payload_len + hdr_len));
|
|
}
|
|
|
|
static err_t ping_send(ping_session_t *session)
|
|
{
|
|
struct icmp_echo_hdr *echohdr; // we allocate a larger buffer to also fit a payload at the end
|
|
struct sockaddr_in addr_to;
|
|
const size_t packet_size = sizeof(struct icmp_echo_hdr) + session->config.payload_size;
|
|
|
|
ESP_LOGD(TAG, "Send ICMP ECHO req to %s", ip4addr_ntoa(&session->config.ip_addr));
|
|
|
|
echohdr = (struct icmp_echo_hdr *) mem_malloc((mem_size_t) packet_size);
|
|
if (!echohdr) {
|
|
return ERR_MEM;
|
|
}
|
|
|
|
ping_prepare_echo(session, echohdr);
|
|
|
|
addr_to.sin_len = sizeof(addr_to);
|
|
addr_to.sin_family = AF_INET;
|
|
addr_to.sin_addr.s_addr = session->config.ip_addr.addr; // ?
|
|
|
|
int ret = sendto(session->sockfd, echohdr, packet_size, 0, (struct sockaddr *) &addr_to, sizeof(addr_to));
|
|
if (ret <= 0) {
|
|
ESP_LOGE(TAG, "ping sendto err %d", ret);
|
|
}
|
|
else {
|
|
session->transmitted++;
|
|
}
|
|
|
|
free(echohdr);
|
|
return (ret > 0 ? ERR_OK : ERR_VAL);
|
|
}
|
|
|
|
static void ping_recv(ping_session_t *session)
|
|
{
|
|
char rxbuf[64];
|
|
int len;
|
|
struct sockaddr_in addr_from;
|
|
struct ip_hdr *iphdr;
|
|
struct icmp_echo_hdr *echohdr = NULL;
|
|
struct timeval begin, end;
|
|
uint64_t micros_begin, micros_end, elapsed_ms;
|
|
|
|
socklen_t fromlen = sizeof(struct sockaddr_in);
|
|
|
|
// Register begin time
|
|
gettimeofday(&begin, NULL); // FIXME this will fail if they are in different days
|
|
|
|
// Receive a response limit size to recv buffer - leftovers will be collected and discarded
|
|
while (0 < (len = recvfrom(session->sockfd, rxbuf, sizeof(rxbuf), 0, (struct sockaddr *) &addr_from, &fromlen))) {
|
|
if (len >= (int) (sizeof(struct ip_hdr) + sizeof(struct icmp_echo_hdr))) {
|
|
// Register end time
|
|
gettimeofday(&end, NULL);
|
|
|
|
/// Get from IP address
|
|
ip4_addr_t fromaddr;
|
|
fromaddr.addr = addr_from.sin_addr.s_addr; // ???
|
|
|
|
// Get echo
|
|
iphdr = (struct ip_hdr *) rxbuf;
|
|
echohdr = (struct icmp_echo_hdr *) (rxbuf + (IPH_HL(iphdr) * 4));
|
|
|
|
// Print ....
|
|
if ((echohdr->id == PING_ID) && (echohdr->seqno == htons(session->ping_seq_num))) {
|
|
session->received++;
|
|
|
|
// Get elapsed time in milliseconds
|
|
micros_begin = (uint64_t) begin.tv_sec * 1000000;
|
|
micros_begin += begin.tv_usec;
|
|
|
|
micros_end = (uint64_t) end.tv_sec * 1000000;
|
|
micros_end += end.tv_usec;
|
|
|
|
elapsed_ms = (micros_end - micros_begin) / 1000;
|
|
|
|
session->last_delay_ms = (uint16_t) elapsed_ms;
|
|
|
|
// Update statistics
|
|
if (elapsed_ms < session->min_time_ms) {
|
|
session->min_time_ms = (uint16_t) elapsed_ms;
|
|
}
|
|
|
|
if (elapsed_ms > session->max_time_ms) {
|
|
session->max_time_ms = (uint16_t) elapsed_ms;
|
|
}
|
|
|
|
// Print ...
|
|
|
|
int seq = ntohs(echohdr->seqno);
|
|
const char *ipa = ip4addr_ntoa(&fromaddr);
|
|
|
|
ESP_LOGD(TAG, "Rx %d bytes from %s: icmp_seq=%d time=%d ms", len, ipa, seq, (int) elapsed_ms);
|
|
|
|
if (session->config.success_cb) {
|
|
session->config.success_cb(len, ipa, seq, (int) elapsed_ms);
|
|
}
|
|
|
|
return;
|
|
}
|
|
else {
|
|
// junk, ignore
|
|
ESP_LOGD(TAG, "Rx %d bytes from %s: junk", len, ip4addr_ntoa(&fromaddr));
|
|
}
|
|
}
|
|
}
|
|
|
|
session->last_delay_ms = 0;
|
|
|
|
if (len < 0) {
|
|
if (session->config.fail_cb) {
|
|
session->config.fail_cb(session->ping_seq_num);
|
|
}
|
|
|
|
ESP_LOGW(TAG, "Request timeout for icmp_seq %d", session->ping_seq_num);
|
|
}
|
|
}
|
|
|
|
esp_err_t ping(const ping_opts_t *opts, ping_result_t *result)
|
|
{
|
|
ping_session_t session = {
|
|
.min_time_ms = UINT16_MAX,
|
|
};
|
|
|
|
if (opts == NULL) {
|
|
ESP_LOGE(TAG, "opts arg is null");
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
if (result == NULL) {
|
|
ESP_LOGE(TAG, "result arg is null");
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
if (opts->count == 0) {
|
|
ESP_LOGE(TAG, "ping count must be > 0");
|
|
}
|
|
|
|
memcpy(&session.config, opts, sizeof(ping_opts_t));
|
|
memset(result, 0, sizeof(ping_result_t));
|
|
|
|
// Create socket
|
|
if ((session.sockfd = socket(AF_INET, SOCK_RAW, IP_PROTO_ICMP)) < 0) {
|
|
ESP_LOGE(TAG, "fail to open socket for ping");
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
result->sockfd = session.sockfd;
|
|
|
|
// Setup socket
|
|
struct timeval tout;
|
|
tout.tv_sec = opts->timeout_ms / 1000;
|
|
tout.tv_usec = (opts->timeout_ms % 1000) * 1000;
|
|
|
|
if (setsockopt(session.sockfd, SOL_SOCKET, SO_RCVTIMEO, &tout, sizeof(tout)) < 0) {
|
|
closesocket(session.sockfd);
|
|
session.sockfd = -1;
|
|
result->sockfd = -1;
|
|
ESP_LOGE(TAG, "fail to set ping socket rx timeout");
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
if (setsockopt(session.sockfd, SOL_SOCKET, SO_SNDTIMEO, &tout, sizeof(tout)) < 0) {
|
|
closesocket(session.sockfd);
|
|
session.sockfd = -1;
|
|
result->sockfd = -1;
|
|
ESP_LOGE(TAG, "fail to set ping socket tx timeout");
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
ESP_LOGD(TAG, "Pinging %s: %d data bytes", ip4addr_ntoa(&opts->ip_addr), opts->payload_size);
|
|
|
|
while (session.ping_seq_num < opts->count) {
|
|
if (ping_send(&session) == ERR_OK) {
|
|
ping_recv(&session);
|
|
}
|
|
if (session.ping_seq_num < opts->count) {
|
|
// subtract the wait time from the requested wait interval
|
|
int wait_time = opts->interval_ms - session.last_delay_ms;
|
|
if (wait_time >= 0) { // if 0, just yields
|
|
vTaskDelay(wait_time / portTICK_PERIOD_MS);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (session.sockfd > 0) {
|
|
closesocket(session.sockfd);
|
|
session.sockfd = -1;
|
|
result->sockfd = -1;
|
|
}
|
|
|
|
result->sent = session.transmitted;
|
|
result->received = session.received;
|
|
result->min_time_ms = session.min_time_ms;
|
|
result->max_time_ms = session.max_time_ms;
|
|
result->loss_pt = (float) ((
|
|
((float) session.transmitted - (float) session.received)
|
|
/ (float) session.transmitted
|
|
) * 100.0);
|
|
|
|
ESP_LOGD(TAG, "%d tx, %d rx, %.1f%% loss, latency min %d ms, max %d ms",
|
|
result->sent,
|
|
result->received,
|
|
result->loss_pt,
|
|
result->min_time_ms,
|
|
result->max_time_ms);
|
|
|
|
return ESP_OK;
|
|
}
|
|
|