Air quality sensor
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.
esp-airsensor/components/ping/src/ping.c

263 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;
}