update espnow base component

esphome/components/espnow/__init__.py

@@ -10,14 +10,14 @@ espnow_ns = cg.esphome_ns.namespace("espnow")
 ESPNowComponent = espnow_ns.class_("ESPNowComponent", cg.Component)
 ESPNowListener = espnow_ns.class_("ESPNowListener")
 
-ESPNowPackage = espnow_ns.class_("ESPNowPackage")
+ESPNowPacket = espnow_ns.class_("ESPNowPacket")
-ESPNowPackagePtrConst = ESPNowPackage.operator("ptr")  # .operator("const")
+ESPNowPacketPtrConst = ESPNowPacket.operator("ptr")  # .operator("const")
 
 ESPNowInterface = espnow_ns.class_(
     "ESPNowInterface", cg.Component, cg.Parented.template(ESPNowComponent)
 )
 
-ESPNowSendTrigger = espnow_ns.class_("ESPNowSendTrigger", automation.Trigger.template())
+ESPNowSentTrigger = espnow_ns.class_("ESPNowSentTrigger", automation.Trigger.template())
 ESPNowReceiveTrigger = espnow_ns.class_(
     "ESPNowReceiveTrigger", automation.Trigger.template()
 )
@@ -30,12 +30,13 @@ NewPeerAction = espnow_ns.class_("NewPeerAction", automation.Action)
 DelPeerAction = espnow_ns.class_("DelPeerAction", automation.Action)
 
 CONF_ESPNOW = "espnow"
-CONF_ON_PACKAGE_RECEIVED = "on_package_received"
+CONF_ON_RECEIVE = "on_receive"
-CONF_ON_PACKAGE_SEND = "on_package_send"
+CONF_ON_SENT = "on_sent"
 CONF_ON_NEW_PEER = "on_new_peer"
 CONF_WIFI_CHANNEL = "wifi_channel"
 CONF_PEERS = "peers"
 CONF_AUTO_ADD_PEER = "auto_add_peer"
+CONF_USE_SENT_CHECK = "use_sent_check"
 
 
 def validate_raw_data(value):
@@ -53,14 +54,15 @@ CONFIG_SCHEMA = cv.Schema(
         cv.GenerateID(): cv.declare_id(ESPNowComponent),
         cv.Optional(CONF_WIFI_CHANNEL, default=0): cv.int_range(0, 14),
         cv.Optional(CONF_AUTO_ADD_PEER, default=False): cv.boolean,
-        cv.Optional(CONF_ON_PACKAGE_RECEIVED): automation.validate_automation(
+        cv.Optional(CONF_USE_SENT_CHECK, default=True): cv.boolean,
+        cv.Optional(CONF_ON_RECEIVE): automation.validate_automation(
             {
                 cv.GenerateID(CONF_TRIGGER_ID): cv.declare_id(ESPNowReceiveTrigger),
             }
         ),
-        cv.Optional(CONF_ON_PACKAGE_SEND): automation.validate_automation(
+        cv.Optional(CONF_ON_SENT): automation.validate_automation(
             {
-                cv.GenerateID(CONF_TRIGGER_ID): cv.declare_id(ESPNowSendTrigger),
+                cv.GenerateID(CONF_TRIGGER_ID): cv.declare_id(ESPNowSentTrigger),
             }
         ),
         cv.Optional(CONF_ON_NEW_PEER): automation.validate_automation(
@@ -78,40 +80,46 @@ async def to_code(config):
     var = cg.new_Pvariable(config[CONF_ID])
     await cg.register_component(var, config)
 
-    if CORE.is_esp8266:
+    if CORE.is_esp32 and CORE.using_arduino:
-        cg.add_library("ESP8266WiFi", None)
-    elif CORE.is_esp32 and CORE.using_arduino:
-        cg.add_library("WiFi", None)
-    elif CORE.is_rp2040:
         cg.add_library("WiFi", None)
 
     cg.add_define("USE_ESPNOW")
 
     cg.add(var.set_wifi_channel(config[CONF_WIFI_CHANNEL]))
     cg.add(var.set_auto_add_peer(config[CONF_AUTO_ADD_PEER]))
+    cg.add(var.set_use_sent_check(config[CONF_USE_SENT_CHECK]))
 
-    for conf in config.get(CONF_ON_PACKAGE_SEND, []):
+    for conf in config.get(CONF_ON_SENT, []):
         trigger = cg.new_Pvariable(conf[CONF_TRIGGER_ID], var)
-        await automation.build_automation(
+        await automation.build_automation(trigger, [(ESPNowPacket, "it")], conf)
-            trigger, [(ESPNowPackagePtrConst, "it")], conf
-        )
 
-    for conf in config.get(CONF_ON_PACKAGE_RECEIVED, []):
+    for conf in config.get(CONF_ON_RECEIVE, []):
         trigger = cg.new_Pvariable(conf[CONF_TRIGGER_ID], var)
-        await automation.build_automation(
+        await automation.build_automation(trigger, [(ESPNowPacket, "it")], conf)
-            trigger, [(ESPNowPackagePtrConst, "it")], conf
-        )
 
     for conf in config.get(CONF_ON_NEW_PEER, []):
         trigger = cg.new_Pvariable(conf[CONF_TRIGGER_ID], var)
         await automation.build_automation(
-            trigger, [(ESPNowPackagePtrConst, "it")], conf
+            trigger, [(ESPNowPacket, "it"), (bool, "status")], conf
         )
 
     for conf in config.get(CONF_PEERS, []):
         cg.add(var.add_peer(conf.as_hex))
 
 
+PROTOCOL_SCHEMA = cv.Schema(
+    {
+        cv.GenerateID(CONF_ESPNOW): cv.use_id(ESPNowComponent),
+    },
+    cv.only_on_esp32,
+).extend(cv.COMPONENT_SCHEMA)
+
+
+async def register_protocol(var, config):
+    now = await cg.get_variable(config[CONF_ESPNOW])
+    cg.add(now.register_protocol(var))
+
+
 @automation.register_action(
     "espnow.send",
     SendAction,

esphome/components/espnow/espnow.cpp

@@ -13,7 +13,7 @@
 #ifdef USE_WIFI
 #include "esphome/components/wifi/wifi_component.h"
 #endif
-
+#include "esphome/core/application.h"
 #include "esphome/core/version.h"
 #include "esphome/core/log.h"
 
@@ -22,6 +22,14 @@ namespace espnow {
 
 static const char *const TAG = "espnow";
 
+static const size_t SEND_BUFFER_SIZE = 200;
+
+static void application_task(void *param) {
+  // delegate onto the application
+  ESPNowComponent *application = (ESPNowComponent *) param;
+  application->runner();
+}
+
 #if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 1)
 typedef struct {
   uint16_t frame_head;
@@ -45,28 +53,36 @@ typedef struct {
 } __attribute__((packed)) espnow_frame_format_t;
 #endif
 
-void ESPNowInterface::setup() { parent_->register_protocol(this); }
+void ESPNowProtocol::setup() { parent_->register_protocol(this); }
 
-ESPNowPackage::ESPNowPackage(const uint64_t mac_address, const std::vector<uint8_t> data) {
+bool ESPNowProtocol::write(const uint64_t mac_address, const uint8_t *data, uint8_t len) {
-  this->mac_address_ = mac_address;
+  ESPNowPacket packet(mac_address, data, len, this->get_app_id());
-  this->data_ = data;
+  return this->parent_->write(packet);
 }
-
+bool ESPNowProtocol::write(const uint64_t mac_address, const std::vector<uint8_t> data) {
-ESPNowPackage::ESPNowPackage(const uint64_t mac_address, const uint8_t *data, size_t len) {
+  ESPNowPacket packet(mac_address, (uint8_t *) data.data(), (uint8_t) data.size(), this->get_app_id());
-  this->mac_address_ = mac_address;
+  return this->parent_->write(packet);
-  this->data_.clear();
+}
-  //  this->data_.insert(this->data_.begin(), len,  *data)
+bool ESPNowProtocol::write(ESPNowPacket packet) {
-  //  std::copy_n(data, len, this->data_.begin());
+  packet.app_id = this->get_app_id();
+  packet.ref_id = this->get_next_ref_id();
+  packet.recalc();
+  return this->parent_->write(packet);
 }
 
 ESPNowComponent::ESPNowComponent() { global_esp_now = this; }
 
-void ESPNowComponent::log_error_(std::string msg, esp_err_t err) { ESP_LOGE(TAG, msg.c_str(), esp_err_to_name(err)); }
-
 void ESPNowComponent::dump_config() {
   ESP_LOGCONFIG(TAG, "esp_now:");
   ESP_LOGCONFIG(TAG, "  MAC Address: " MACSTR, MAC2STR(ESPNOW_ADDR_SELF));
-  // ESP_LOGCONFIG(TAG, "  WiFi Channel: %n", WiFi.channel());
+  ESPNowPacket packet(0x112233445566, (uint8_t *) TAG, 5, 0x111111);
+  ESP_LOGI(TAG, "test: %s |H:%02x%02x%02x  A:%02x%02x%02x %02x  T:%02x  C:%02x%02x S:%02d", packet.to_str().c_str(),
+           packet.content[0], packet.content[1], packet.content[2], packet.content[3], packet.content[4],
+           packet.content[5], packet.content[6], packet.content[7], packet.content[8], packet.content[9], packet.size);
+
+  ESP_LOGI(TAG, "test: A:%06x  R:%02x  C:%04x S:%02d", packet.app_id, packet.ref_id, packet.crc16, packet.size);
+  ESP_LOGI(TAG, "test: is_valid: %s",
+           packet.is_valid() ? "Yes" : "No");  // ESP_LOGCONFIG(TAG, "  WiFi Channel: %n", WiFi.channel());
 }
 
 bool ESPNowComponent::validate_channel_(uint8_t channel) {
@@ -108,13 +124,6 @@ void ESPNowComponent::setup() {
   esp_wifi_set_channel(this->wifi_channel_, WIFI_SECOND_CHAN_NONE);
   esp_wifi_set_promiscuous(false);
 
-  this->send_lock_ = xSemaphoreCreateMutex();
-  if (!this->send_lock_) {
-    ESP_LOGE(TAG, "Create send semaphore mutex fail");
-    this->mark_failed();
-    return;
-  }
-
   esp_err_t err = esp_now_init();
   if (err != ESP_OK) {
     ESP_LOGE(TAG, "esp_now_init failed: %s", esp_err_to_name(err));
@@ -124,39 +133,42 @@ void ESPNowComponent::setup() {
 
   err = esp_now_register_recv_cb(ESPNowComponent::on_data_received);
   if (err != ESP_OK) {
-    this->log_error_("esp_now_register_recv_cb failed: %s", err);
+    ESP_LOGE("esp_now_register_recv_cb failed: %s", esp_err_to_name(err));
     this->mark_failed();
     return;
   }
 
-  err = esp_now_register_send_cb(ESPNowComponent::on_data_send);
+  err = esp_now_register_send_cb(ESPNowComponent::on_data_sent);
   if (err != ESP_OK) {
-    this->log_error_("esp_now_register_send_cb failed: %s", err);
+    ESP_LOGE("esp_now_register_send_cb failed: %s", esp_err_to_name(err));
     this->mark_failed();
     return;
   }
 
   esp_wifi_get_mac(WIFI_IF_STA, ESPNOW_ADDR_SELF);
 
-  ESP_LOGI(TAG, "ESP-NOW add peers.");
   for (auto &address : this->peers_) {
     ESP_LOGI(TAG, "Add peer 0x%s .", format_hex(address).c_str());
     add_peer(address);
   }
-  ESP_LOGI(TAG, "ESP-NOW setup complete");
-}
 
-ESPNowPackage *ESPNowComponent::send_package(ESPNowPackage *package) {
+  this->send_queue_ = xQueueCreate(SEND_BUFFER_SIZE, sizeof(ESPNowPacket));
-  uint8_t mac[6];
+  if (this->send_queue_ == nullptr) {
-  package->mac_bytes((uint8_t *) &mac);
+    ESP_LOGE(TAG, "Failed to create send queue");
-
+    this->mark_failed();
-  if (esp_now_is_peer_exist((uint8_t *) &mac)) {
+    return;
-    this->send_queue_.push(std::move(package));
-  } else {
-    ESP_LOGW(TAG, "Peer does not exist cant send this package: %02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2],
-             mac[3], mac[4], mac[5]);
   }
-  return package;
+
+  this->receive_queue_ = xQueueCreate(SEND_BUFFER_SIZE, sizeof(ESPNowPacket));
+  if (this->receive_queue_ == nullptr) {
+    ESP_LOGE(TAG, "Failed to create receive queue");
+    this->mark_failed();
+    return;
+  }
+
+  xTaskCreate(application_task, "espnow_task", 8192, this, 2, &this->espnow_task_handle_);
+
+  ESP_LOGI(TAG, "ESP-NOW setup complete");
 }
 
 esp_err_t ESPNowComponent::add_peer(uint64_t addr) {
@@ -167,13 +179,11 @@ esp_err_t ESPNowComponent::add_peer(uint64_t addr) {
     uint8_t mac[6];
     this->del_peer(addr);
 
-    uint64_to_addr(addr, (uint8_t *) &mac);
-
     esp_now_peer_info_t peerInfo = {};
     memset(&peerInfo, 0, sizeof(esp_now_peer_info_t));
     peerInfo.channel = this->wifi_channel_;
     peerInfo.encrypt = false;
-    memcpy(peerInfo.peer_addr, mac, 6);
+    memcpy((void *) peerInfo.peer_addr, (void *) &addr, 6);
 
     return esp_now_add_peer(&peerInfo);
   }
@@ -181,97 +191,42 @@ esp_err_t ESPNowComponent::add_peer(uint64_t addr) {
 
 esp_err_t ESPNowComponent::del_peer(uint64_t addr) {
   uint8_t mac[6];
-  uint64_to_addr(addr, (uint8_t *) &mac);
+  memcpy((void *) &mac, (void *) &addr, 6);
   if (esp_now_is_peer_exist((uint8_t *) &mac))
     return esp_now_del_peer((uint8_t *) &mac);
   return ESP_OK;
 }
 
-void ESPNowComponent::on_package_received(ESPNowPackage *package) {
+ESPNowDefaultProtocol *ESPNowComponent::get_defaultProtocol_() {
-  for (auto *protocol : this->protocols_) {
+  if (this->protocols_[ESPNOW_DEFAULT_APP_ID] == nullptr) {
-    if (protocol->on_package_received(package)) {
+    ESPNowDefaultProtocol *tmp = new ESPNowDefaultProtocol();
-      return;
+    this->protocols_[ESPNOW_DEFAULT_APP_ID] = tmp;
+    this->register_protocol(tmp);
   }
-  }
+  return (ESPNowDefaultProtocol *) this->protocols_[ESPNOW_DEFAULT_APP_ID];
-  this->on_package_receved_.call(package);
 }
 
-void ESPNowComponent::on_package_send(ESPNowPackage *package) {
+void ESPNowComponent::on_receive_(ESPNowPacket packet) {
-  for (auto *protocol : this->protocols_) {
+  if (this->protocols_[packet.app_id] == nullptr) {
-    if (protocol->on_package_send(package)) {
+    ESP_LOGE(TAG, "Protocol for '%06x' is not registered", packet.app_id);
-      return;
+  } else {
-    }
+    this->protocols_[packet.app_id]->on_receive(packet);
-  }
-  this->on_package_send_.call(package);
-}
-
-void ESPNowComponent::on_new_peer(ESPNowPackage *package) {
-  for (auto *protocol : this->protocols_) {
-    if (protocol->on_new_peer(package)) {
-      return;
-    }
-  }
-  this->on_new_peer_.call(package);
-}
-
-void ESPNowComponent::unHold_send_(uint64_t mac) {
-  for (ESPNowPackage *package : this->send_queue_) {
-    if (package->is_holded() && package->mac_address() == mac) {
-      package->reset_counter();
-    }
   }
 }
 
-void ESPNowComponent::loop() {
+void ESPNowComponent::on_sent_(ESPNowPacket packet, bool status) {
-  if (!send_queue_.empty() && this->can_send_ && !this->status_has_warning()) {
+  if (this->protocols_[packet.app_id] == nullptr) {
-    ESPNowPackage *package = this->send_queue_.front();
+    ESP_LOGE(TAG, "Protocol for '%06x' is not registered", packet.app_id);
-    ESPNowPackage *front = package;
+  } else {
-    while (package->is_holded()) {
+    this->protocols_[packet.app_id]->on_sent(packet, status);
-      this->send_queue_.pop();
-      this->send_queue_.push(package);
-      package = this->send_queue_.front();
-      if (front == package)
-        break;
   }
+}
 
-    if (!package->is_holded()) {
+void ESPNowComponent::on_new_peer_(ESPNowPacket packet) {
-      if (package->get_counter() == 5) {
+  if (this->protocols_[packet.app_id] == nullptr) {
-        this->status_set_warning("to many send retries. Stopping sending until new package received.");
+    ESP_LOGE(TAG, "Protocol for '%06x' is not registered", packet.app_id);
   } else {
-        uint8_t mac_address[6];
+    this->protocols_[packet.app_id]->on_new_peer(packet);
-        package->mac_bytes((uint8_t *) &mac_address);
-        esp_err_t err = esp_now_send(mac_address, package->data().data(), package->data().size());
-        package->inc_counter();
-        if (err != ESP_OK) {
-          this->log_error_("esp_now_init failed: %s", err);
-          this->send_queue_.pop();
-          this->send_queue_.push(package);
-        } else {
-          this->can_send_ = false;
-        }
-      }
-    }
-  }
-
-  while (!receive_queue_.empty()) {
-    ESPNowPackage *package = std::move(this->receive_queue_.front());
-    this->receive_queue_.pop();
-    uint8_t mac[6];
-    package->mac_bytes((uint8_t *) &mac);
-    if (!esp_now_is_peer_exist((uint8_t *) &mac)) {
-      if (this->auto_add_peer_) {
-        this->add_peer(addr_to_uint64((uint8_t *) &mac));
-      } else {
-        this->on_new_peer(package);
-      }
-    }
-    if (esp_now_is_peer_exist((uint8_t *) &mac)) {
-      this->unHold_send_(package->mac_address());
-      this->on_package_received(package);
-    } else {
-      ESP_LOGW(TAG, "Peer does not exist can't handle this package: %02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1],
-               mac[2], mac[3], mac[4], mac[5]);
-    }
   }
 }
 
@@ -282,44 +237,151 @@ void ESPNowComponent::on_data_received(const esp_now_recv_info_t *recv_info, con
 void ESPNowComponent::on_data_received(const uint8_t *addr, const uint8_t *data, int size)
 #endif
 {
+  ESPNowPacket packet;
   wifi_pkt_rx_ctrl_t *rx_ctrl = NULL;
 
 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 1)
   uint8_t *addr = recv_info->src_addr;
+  packet.broadcast = (*recv_info->des_addr == ESPNOW_BROADCAST_ADDR);
   rx_ctrl = recv_info->rx_ctrl;
 #else
   wifi_promiscuous_pkt_t *promiscuous_pkt =
       (wifi_promiscuous_pkt_t *) (data - sizeof(wifi_pkt_rx_ctrl_t) - sizeof(espnow_frame_format_t));
   rx_ctrl = &promiscuous_pkt->rx_ctrl;
 #endif
+  packet.set_mac((espnow_addr_t *) addr);
+  packet.rssi = rx_ctrl->rssi;
+  memcpy((void *) &packet.content, (void *) data, size);
+  packet.size = size - 10;
+  packet.timestamp = rx_ctrl->timestamp;
+  ESP_LOGVV(TAG, "Read: %s |H:%02x%02x%02x  A:%02x%02x%02x %02x  T:%02x  C:%02x%02x S:%02d", packet.to_str().c_str(),
+            packet.content[0], packet.content[1], packet.content[2], packet.content[3], packet.content[4],
+            packet.content[5], packet.content[6], packet.content[7], packet.content[8], packet.content[9], packet.size);
 
-  ESPNowPackage *package = new ESPNowPackage(addr_to_uint64(addr), data, size);
+  if (packet.is_valid()) {
-
+    xQueueSendToBack(global_esp_now->receive_queue_, &packet, 10);
-#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 0, 1)
+  } else {
-  package->is_broadcast(addr_to_uint64(recv_info->des_addr) == ESPNOW_BROADCAST_ADDR);
+    ESP_LOGE(TAG, "Invalid ESP-NOW packet received (CRC)");
-#endif
+  }
-
-  package->rssi(rx_ctrl->rssi);
-  package->timestamp(rx_ctrl->timestamp);
-  global_esp_now->push_receive_package_(package);
 }
 
-void ESPNowComponent::on_data_send(const uint8_t *mac_addr, esp_now_send_status_t status) {
+bool ESPNowComponent::write(ESPNowPacket packet) {
-  ESPNowPackage *package = global_esp_now->send_queue_.front();
+  ESP_LOGVV(TAG, "Write: %s |H:%02x%02x%02x  A:%02x%02x%02x %02x  T:%02x  C:%02x%02x S:%02d", packet.to_str().c_str(),
+            packet.content[0], packet.content[1], packet.content[2], packet.content[3], packet.content[4],
+            packet.content[5], packet.content[6], packet.content[7], packet.content[8], packet.content[9], packet.size);
   espnow_addr_t mac;
-  uint64_to_addr(package->mac_address(), mac);
+  packet.get_mac(&mac);
-  if (status != ESP_OK) {
+  if (this->is_failed()) {
-    ESP_LOGE(TAG, "on_data_send failed");
+    ESP_LOGE(TAG, "Cannot send espnow packet, espnow failed to setup");
-  } else if (std::memcmp(mac, mac_addr, 6) != 0) {
+  } else if (this->send_queue_full()) {
-    ESP_LOGE(TAG, "on_data_send Invalid mac address.");
+    ESP_LOGE(TAG, "Send Buffer Out of Memory.");
-    ESP_LOGW(TAG, "expected: %02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+  } else if (!esp_now_is_peer_exist((uint8_t *) &mac)) {
-    ESP_LOGW(TAG, "returned: %02X:%02X:%02X:%02X:%02X:%02X", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3],
+    ESP_LOGW(TAG, "Peer not registered: 0x%s.", packet.to_str().c_str());
-             mac_addr[4], mac_addr[5]);
+  } else if (!packet.is_valid()) {
+    ESP_LOGW(TAG, "Packet is invalid. maybe you need to ::recalc(). the packat before writing.");
+  } else if (this->use_sent_check_) {
+    xQueueSendToBack(this->send_queue_, &packet, 10);
+    ESP_LOGVV(TAG, "Send (0x%04x.%d): 0x%s. Buffer Used: %d", packet.ref_id, packet.retrys,
+              format_hex(packet.mac64).c_str(), this->send_queue_used());
+    return true;
   } else {
-    global_esp_now->on_package_send(package);
+    esp_err_t err = esp_now_send((uint8_t *) &mac, (uint8_t *) &packet.content, packet.size + 10);
-    global_esp_now->send_queue_.pop();
+    ESP_LOGVV(TAG, "S: 0x%04x.%d B: %d%s.", packet.ref_id, packet.retrys, this->send_queue_used(),
+              (err == ESP_OK) ? "" : " FAILED");
+    this->defer([this, packet, err]() { this->on_sent_(packet, err == ESP_OK); });
   }
-  global_esp_now->can_send_ = true;
+  return false;
+}
+
+void ESPNowComponent::loop() {
+  // runner();
+}
+
+void ESPNowComponent::runner() {
+  ESPNowPacket packet;
+
+  for (;;) {
+    if (xQueueReceive(this->receive_queue_, &packet, (TickType_t) 1) == pdTRUE) {
+      espnow_addr_t mac;
+      packet.get_mac(&mac);
+
+      if (!esp_now_is_peer_exist((uint8_t *) &mac)) {
+        if (this->auto_add_peer_) {
+          this->add_peer(packet.mac64);
+        } else {
+          this->defer([this, packet]() { this->on_new_peer_(packet); });
+          continue;
+        }
+      }
+      if (esp_now_is_peer_exist((uint8_t *) &mac)) {
+        this->defer([this, packet]() { this->on_receive_(packet); });
+      } else {
+        ESP_LOGE(TAG, "Peer not registered: %s", format_hex(packet.mac64).c_str());
+      }
+    }
+    if (xQueueReceive(this->send_queue_, &packet, (TickType_t) 1) == pdTRUE) {
+      if (this->is_locked()) {
+        if (millis() - packet.timestamp > 1000) {
+          if (packet.retrys == 6) {
+            ESP_LOGW(TAG, "To many send retries. Packet dropped. 0x%04x", packet.ref_id);
+            this->unlock();
+            continue;
+          } else {
+            ESP_LOGE(TAG, "TimeOut (0x%04x.%d).", packet.ref_id, packet.retrys);
+            packet.retry();
+          }
+          this->unlock();
+        }
+      } else {
+        packet.retry();
+        if (packet.retrys == 6) {
+          ESP_LOGW(TAG, "To many send retries. Packet dropped. 0x%04x", packet.ref_id);
+          // continue;
+          return;
+        } else {
+          packet.timestamp = millis();
+          this->lock();
+          espnow_addr_t mac;
+          packet.get_mac(&mac);
+
+          esp_err_t err = esp_now_send((uint8_t *) &mac, packet.data, packet.size + 10);
+
+          if (err == ESP_OK) {
+            ESP_LOGV(TAG, "S: 0x%04x.%d. Wait for conformation. M: %s", packet.ref_id, packet.retrys,
+                     packet.to_str().c_str());
+          } else {
+            ESP_LOGE(TAG, "S: 0x%04x.%d B: %d.", packet.ref_id, packet.retrys, this->send_queue_used());
+            this->unlock();
+          }
+        }
+      }
+      xQueueSendToFront(this->send_queue_, &packet, 10 / portTICK_PERIOD_MS);
+    }
+  }
+}
+
+void ESPNowComponent::on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+  ESPNowPacket packet;
+  if (!global_esp_now->use_sent_check_) {
+    return;
+  }
+  uint64_t mac64 = packet.to_mac64((espnow_addr_t *) mac_addr);
+  if (xQueueReceive(global_esp_now->send_queue_, &packet, 10 / portTICK_PERIOD_MS) == pdTRUE) {
+    if (status != ESP_OK) {
+      ESP_LOGE(TAG, "sent packet failed (0x%04x.%d)", packet.ref_id, packet.retrys);
+    } else if (packet.mac64 != mac64) {
+      ESP_LOGE(TAG, " Invalid mac address. (0x%04x.%d) expected: %s got %s", packet.ref_id, packet.retrys,
+               packet.to_str().c_str(), packet.to_str(mac64));
+    } else {
+      ESP_LOGV(TAG, "Confirm sent (0x%04x.%d)", packet.ref_id, packet.retrys);
+      global_esp_now->unlock();
+      global_esp_now->defer([packet]() { global_esp_now->on_sent_(packet, true); });
+      return;
+    }
+    global_esp_now->defer([packet]() { global_esp_now->on_sent_(packet, false); });
+    xQueueSendToFront(global_esp_now->send_queue_, &packet, 10 / portTICK_PERIOD_MS);
+  }
+  global_esp_now->unlock();
 }
 
 ESPNowComponent *global_esp_now = nullptr;

esphome/components/espnow/espnow.h

@@ -1,119 +1,79 @@
 #pragma once
 
-// #if defined(USE_ESP32)
+#if defined(USE_ESP32)
 
 #include "esphome/core/automation.h"
 #include "esphome/core/component.h"
 #include "esphome/core/helpers.h"
-
+#include "espnow_packet.h"
 #include <esp_now.h>
 
 #include <array>
 #include <memory>
 #include <queue>
 #include <vector>
+#include <mutex>
+#include <map>
 
 namespace esphome {
 namespace espnow {
 
-typedef uint8_t espnow_addr_t[6];
-
-static const uint64_t ESPNOW_BROADCAST_ADDR = 0xFFFFFFFFFFFF;
-static espnow_addr_t ESPNOW_ADDR_SELF = {0};
-
-static void uint64_to_addr(uint64_t address, uint8_t *bd_addr) {
-  *(bd_addr + 0) = (address >> 40) & 0xff;
-  *(bd_addr + 1) = (address >> 32) & 0xff;
-  *(bd_addr + 2) = (address >> 24) & 0xff;
-  *(bd_addr + 3) = (address >> 16) & 0xff;
-  *(bd_addr + 4) = (address >> 8) & 0xff;
-  *(bd_addr + 5) = (address >> 0) & 0xff;
-}
-
-static uint64_t addr_to_uint64(const uint8_t *address) {
-  uint64_t u = 0;
-  u |= uint64_t(*(address + 0) & 0xFF) << 40;
-  u |= uint64_t(*(address + 1) & 0xFF) << 32;
-  u |= uint64_t(*(address + 2) & 0xFF) << 24;
-  u |= uint64_t(*(address + 3) & 0xFF) << 16;
-  u |= uint64_t(*(address + 4) & 0xFF) << 8;
-  u |= uint64_t(*(address + 5) & 0xFF) << 0;
-  return u;
-}
-
 class ESPNowComponent;
 
-template<typename T, typename Container = std::deque<T> > class iterable_queue : public std::queue<T, Container> {
+static const uint32_t ESPNOW_DEFAULT_APP_ID = 0x11CFAF;
+
+class ESPNowProtocol : public Component, public Parented<ESPNowComponent> {
  public:
-  typedef typename Container::iterator iterator;
+  ESPNowProtocol(){};
-  typedef typename Container::const_iterator const_iterator;
-
-  iterator begin() { return this->c.begin(); }
-  iterator end() { return this->c.end(); }
-  const_iterator begin() const { return this->c.begin(); }
-  const_iterator end() const { return this->c.end(); }
-};
-
-class ESPNowPackage {
- public:
-  ESPNowPackage(const uint64_t mac_address, const std::vector<uint8_t> data);
-  ESPNowPackage(const uint64_t mac_address, const uint8_t *data, size_t len);
-
-  uint64_t mac_address() { return this->mac_address_ == 0 ? ESPNOW_BROADCAST_ADDR : this->mac_address_; }
-
-  void mac_bytes(uint8_t *mac_addres) {
-    uint64_t mac = this->mac_address_ == 0 ? ESPNOW_BROADCAST_ADDR : this->mac_address_;
-    uint64_to_addr(mac, mac_addres);
-  }
-
-  std::vector<uint8_t> data() { return data_; }
-
-  uint8_t get_counter() { return send_count_; }
-  void inc_counter() {
-    send_count_ = send_count_ + 1;
-    if (send_count_ > 5 && !is_holded_) {
-      set_holding();
-    }
-  }
-  void reset_counter() {
-    send_count_ = 0;
-    del_holding();
-  }
-
-  void is_broadcast(bool value) { this->is_broadcast_ = value; }
-  bool is_broadcast() const { return this->is_broadcast_; }
-
-  void timestamp(uint32_t value) { this->timestamp_ = value; }
-  uint32_t timestamp() { return this->timestamp_; }
-
-  void rssi(int8_t rssi) { this->rssi_ = rssi; }
-  int8_t rssi() { return this->rssi_; }
-
-  bool is_holded() { return this->is_holded_; }
-  void set_holding() { this->is_holded_ = true; }
-  void del_holding() { this->is_holded_ = false; }
-
- protected:
-  uint64_t mac_address_{0};
-  std::vector<uint8_t> data_;
-
-  uint8_t send_count_{0};
-  bool is_broadcast_{false};
-  uint32_t timestamp_{0};
-  uint8_t rssi_{0};
-
-  bool is_holded_{false};
-};
-
-class ESPNowInterface : public Component, public Parented<ESPNowComponent> {
- public:
-  ESPNowInterface(){};
 
   void setup() override;
 
-  virtual bool on_package_received(ESPNowPackage *package) { return false; };
+  virtual void on_receive(ESPNowPacket packet) { return; };
-  virtual bool on_package_send(ESPNowPackage *package) { return false; };
+  virtual void on_sent(ESPNowPacket packet, bool status) { return; };
-  virtual bool on_new_peer(ESPNowPackage *package) { return false; };
+  virtual void on_new_peer(ESPNowPacket packet) { return; };
+
+  virtual uint32_t get_app_id() = 0;
+  uint8_t get_next_ref_id() { return next_ref_id_++; }
+
+  bool write(const uint64_t mac_address, const uint8_t *data, uint8_t len);
+  bool write(const uint64_t mac_address, const std::vector<uint8_t> data);
+  bool write(ESPNowPacket packet);
+
+ protected:
+  uint8_t next_ref_id_{0};
+};
+
+class ESPNowDefaultProtocol : public ESPNowProtocol {
+ public:
+  bool on_receive(ESPNowPacket packet) {
+    this->on_receive_.call(packet);
+    return true;
+  };
+  bool on_sent(ESPNowPacket packet, bool status) {
+    this->on_sent_.call(packet, status);
+    return true;
+  };
+  bool on_new_peer(ESPNowPacket packet) {
+    this->on_new_peer_.call(packet);
+    return true;
+  };
+
+  uint32_t get_app_id() override { return ESPNOW_DEFAULT_APP_ID; };
+
+  void add_on_sent_callback(std::function<void(ESPNowPacket, bool status)> &&callback) {
+    this->on_sent_.add(std::move(callback));
+  }
+  void add_on_receive_callback(std::function<void(ESPNowPacket)> &&callback) {
+    this->on_receive_.add(std::move(callback));
+  }
+  void add_on_peer_callback(std::function<void(ESPNowPacket)> &&callback) {
+    this->on_new_peer_.add(std::move(callback));
+  }
+
+ protected:
+  CallbackManager<void(ESPNowPacket, bool)> on_sent_;
+  CallbackManager<void(ESPNowPacket)> on_receive_;
+  CallbackManager<void(ESPNowPacket)> on_new_peer_;
 };
 
 class ESPNowComponent : public Component {
@@ -126,77 +86,62 @@ class ESPNowComponent : public Component {
   static void on_data_received(const uint8_t *addr, const uint8_t *data, int size);
 #endif
 
-  static void on_data_send(const uint8_t *mac_addr, esp_now_send_status_t status);
+  static void on_data_sent(const uint8_t *mac_addr, esp_now_send_status_t status);
 
   void dump_config() override;
+
   float get_setup_priority() const override { return -100; }
 
+  void set_wifi_channel(uint8_t channel) { this->wifi_channel_ = channel; }
+  void set_auto_add_peer(bool value) { this->auto_add_peer_ = value; }
+  void set_use_sent_check(bool value) { this->use_sent_check_ = value; }
+
   void setup() override;
 
+  void runner();
   void loop() override;
-  void set_wifi_channel(uint8_t channel) { this->wifi_channel_ = channel; }
 
-  ESPNowPackage *send_package(const uint64_t mac_address, const uint8_t *data, int len) {
+  bool write(ESPNowPacket packet);
-    //  ESPNowPackage * package = new ESPNowPackage(mac_address, data, len);
-    //  return this->send_package(package);
-    return nullptr;
-  }
 
-  ESPNowPackage *send_package(const uint64_t mac_address, const std::vector<uint8_t> data) {
+  void register_protocol(ESPNowProtocol *protocol) {
-    ESPNowPackage *package = new ESPNowPackage(mac_address, data);
-    return this->send_package(package);
-  }
-
-  ESPNowPackage *send_package(ESPNowPackage *package);
-
-  void add_on_package_send_callback(std::function<void(ESPNowPackage *)> &&callback) {
-    this->on_package_send_.add(std::move(callback));
-  }
-
-  void add_on_package_receive_callback(std::function<void(ESPNowPackage *)> &&callback) {
-    this->on_package_receved_.add(std::move(callback));
-  }
-
-  void add_on_peer_callback(std::function<void(ESPNowPackage *)> &&callback) {
-    this->on_new_peer_.add(std::move(callback));
-  }
-
-  void register_protocol(ESPNowInterface *protocol) {
     protocol->set_parent(this);
-    this->protocols_.push_back(protocol);
+    this->protocols_[protocol->get_app_id()] = std::move(protocol);
   }
 
   esp_err_t add_peer(uint64_t addr);
   esp_err_t del_peer(uint64_t addr);
 
-  void set_auto_add_peer(bool value) { this->auto_add_peer_ = value; }
+  bool send_queue_empty() { return uxQueueMessagesWaiting(this->send_queue_) == 0; }
+  bool send_queue_full() { return uxQueueSpacesAvailable(this->send_queue_) == 0; }
+  size_t send_queue_used() { return uxQueueMessagesWaiting(this->send_queue_); }
+  size_t send_queue_free() { return uxQueueSpacesAvailable(this->send_queue_); }
 
-  void on_package_received(ESPNowPackage *package);
+  void lock() { this->lock_ = true; }
-  void on_package_send(ESPNowPackage *package);
+  bool is_locked() { return this->lock_; }
-  void on_new_peer(ESPNowPackage *package);
+  void unlock() { this->lock_ = false; }
 
-  void log_error_(std::string msg, esp_err_t err);
+  ESPNowDefaultProtocol *get_defaultProtocol_();
 
  protected:
-  void unHold_send_(uint64_t mac);
-  void push_receive_package_(ESPNowPackage *package) { this->receive_queue_.push(std::move(package)); }
   bool validate_channel_(uint8_t channel);
+
   uint8_t wifi_channel_{0};
+
   bool auto_add_peer_{false};
+  bool use_sent_check_{true};
+  bool lock_{false};
 
-  CallbackManager<void(ESPNowPackage *)> on_package_send_;
+  void on_receive_(ESPNowPacket packet);
-  CallbackManager<void(ESPNowPackage *)> on_package_receved_;
+  void on_sent_(ESPNowPacket packet, bool status);
-  CallbackManager<void(ESPNowPackage *)> on_new_peer_;
+  void on_new_peer_(ESPNowPacket packet);
 
-  iterable_queue<ESPNowPackage *> receive_queue_{};
+  QueueHandle_t receive_queue_{};
-  iterable_queue<ESPNowPackage *> send_queue_{};
+  QueueHandle_t send_queue_{};
 
-  std::vector<ESPNowInterface *> protocols_{};
+  std::map<uint32_t, ESPNowProtocol *> protocols_{};
   std::vector<uint64_t> peers_{};
 
-  SemaphoreHandle_t send_lock_ = NULL;
+  TaskHandle_t espnow_task_handle_{nullptr};
-
-  bool can_send_{true};
 };
 
 template<typename... Ts> class SendAction : public Action<Ts...>, public Parented<ESPNowComponent> {
@@ -215,10 +160,10 @@ template<typename... Ts> class SendAction : public Action<Ts...>, public Parente
     auto mac = this->mac_.value(x...);
 
     if (this->static_) {
-      this->parent_->send_package(mac, this->data_static_);
+      this->parent_->get_defaultProtocol_()->write(mac, this->data_static_);
     } else {
       auto val = this->data_func_(x...);
-      this->parent_->send_package(mac, val);
+      this->parent_->get_defaultProtocol_()->write(mac, val);
     }
   }
 
@@ -253,24 +198,25 @@ template<typename... Ts> class DelPeerAction : public Action<Ts...>, public Pare
   TemplatableValue<uint64_t, Ts...> mac_{};
 };
 
-class ESPNowSendTrigger : public Trigger<ESPNowPackage *> {
+class ESPNowSentTrigger : public Trigger<ESPNowPacket, bool> {
  public:
-  explicit ESPNowSendTrigger(ESPNowComponent *parent) {
+  explicit ESPNowSentTrigger(ESPNowComponent *parent) {
-    parent->add_on_package_send_callback([this](ESPNowPackage *value) { this->trigger(value); });
+    parent->get_defaultProtocol_()->add_on_sent_callback(
+        [this](ESPNowPacket value, bool status) { this->trigger(value, status); });
   }
 };
 
-class ESPNowReceiveTrigger : public Trigger<ESPNowPackage *> {
+class ESPNowReceiveTrigger : public Trigger<ESPNowPacket> {
  public:
   explicit ESPNowReceiveTrigger(ESPNowComponent *parent) {
-    parent->add_on_package_receive_callback([this](ESPNowPackage *value) { this->trigger(value); });
+    parent->get_defaultProtocol_()->add_on_receive_callback([this](ESPNowPacket value) { this->trigger(value); });
   }
 };
 
-class ESPNowNewPeerTrigger : public Trigger<ESPNowPackage *> {
+class ESPNowNewPeerTrigger : public Trigger<ESPNowPacket> {
  public:
   explicit ESPNowNewPeerTrigger(ESPNowComponent *parent) {
-    parent->add_on_peer_callback([this](ESPNowPackage *value) { this->trigger(value); });
+    parent->get_defaultProtocol_()->add_on_peer_callback([this](ESPNowPacket value) { this->trigger(value); });
   }
 };
 
@@ -279,4 +225,4 @@ extern ESPNowComponent *global_esp_now;
 }  // namespace espnow
 }  // namespace esphome
 
-// #endif
+#endif

esphome/components/espnow/espnow_packet.h

@@ -0,0 +1,138 @@
+#pragma once
+
+// #if defined(USE_ESP32)
+
+#include "esphome/core/automation.h"
+#include "esphome/core/component.h"
+#include "esphome/core/helpers.h"
+#include "esphome/core/log.h"
+
+#include <esp_now.h>
+#include "esp_crc.h"
+
+#include <array>
+#include <memory>
+#include <queue>
+#include <vector>
+
+namespace esphome {
+namespace espnow {
+
+typedef uint8_t espnow_addr_t[6];
+
+static const uint64_t ESPNOW_BROADCAST_ADDR = 0xFFFFFFFFFFFF;
+static espnow_addr_t ESPNOW_ADDR_SELF = {0};
+static const uint8_t MAX_ESPNOW_DATA_SIZE = 240;
+#define TRANSPORT_HEADER_SIZE 3
+
+static const uint8_t transport_header[TRANSPORT_HEADER_SIZE] = {0xC1, 0x99, 0x83};
+
+template<typename... Args> std::string string_format(const std::string &format, Args... args) {
+  int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + 1;  // Extra space for '\0'
+  if (size_s <= 0) {
+    return ("Error during formatting.");
+  }
+  auto size = static_cast<size_t>(size_s);
+  std::unique_ptr<char[]> buf(new char[size]);
+  std::snprintf(buf.get(), size, format.c_str(), args...);
+  return std::string(buf.get(), buf.get() + size - 1);  // We don't want the '\0' inside
+}
+
+static uint8_t last_ref_id = 0;
+
+struct ESPNowPacket {
+  uint64_t mac64 = 0;
+  uint8_t size = 0;
+  uint8_t rssi = 0;
+  uint8_t retrys : 4;
+  uint8_t is_broadcast : 1;
+  uint8_t dummy : 3;
+  uint32_t timestamp = 0;
+
+  union {
+    uint8_t content[MAX_ESPNOW_DATA_SIZE + 11];
+    struct {
+      uint8_t header[3] = {0xC1, 0x99, 0x83};
+      uint32_t app_id = 0xFFFFFF;
+      uint1_t ref_id = 0x99;
+      uint8_t crc16 = 0x1234;
+      uint8_t data[MAX_ESPNOW_DATA_SIZE];
+      uint8_t space = 0;
+    } __attribute__((packed));
+  };
+
+  inline ESPNowPacket() ESPHOME_ALWAYS_INLINE : retrys(0) {}
+
+  inline void info(std::string place) {
+    ESP_LOGVV("Packet", "%s: M:%s A:0x%06x R:0x%02x C:0x%04x S:%02x", place.c_str(), this->to_str().c_str(),
+              this->app_id, this->ref_id, this->random, this->size);
+  }
+
+  inline ESPNowPacket(const uint64_t mac64_, const uint8_t *data_, uint8_t size_,
+                      uint32_t app_id_) ESPHOME_ALWAYS_INLINE : mac64(mac64_),
+                                                                size(size_),
+                                                                app_id(app_id_),
+                                                                retrys(0) {
+    if (mac64_ == 0)
+      this->mac64 = ESPNOW_BROADCAST_ADDR;
+    this->is_broadcast = this->mac64 == ESPNOW_BROADCAST_ADDR;
+
+    this->ref_id = 0;
+
+    this->size = std::min(MAX_ESPNOW_DATA_SIZE, size_);
+    std::memcpy(&data, (uint8_t *) data_, this->size);
+
+    this->data[this->size + 1] = 0;
+    this->recalc();
+    this->info("create");
+  }
+
+  inline void get_mac(espnow_addr_t *mac_addres) { std::memcpy(mac_addres, &mac64, 6); }
+  inline void set_mac(espnow_addr_t *mac_addres) { this->mac64 = this->to_mac64(mac_addres); }
+
+  uint64_t to_mac64(espnow_addr_t *mac_addres) {
+    uint64_t result;
+    std::memcpy(&result, mac_addres, 6);
+    return result;
+  }
+
+  void retry() {
+    if (this->retrys < 7) {
+      retrys = retrys + 1;
+    }
+  }
+
+  inline void recalc() {
+    random = 0;
+    random = esp_crc16_le(ref_id, (uint8_t *) &content, 10 + size);
+  }
+
+  bool is_valid() {
+    uint16_t crc = random;
+    recalc();
+    bool valid = (std::memcmp(&header, &transport_header, 3) == 0);
+    valid &= (this->app_id != 0);
+    valid &= (this->random == crc);
+    if (!valid) {
+      ESP_LOGV("Packet", "Invalid H:%02x%02x%02x A:%06x R:%02x C:%04x ipv. %04x, %d&%d&%d=%d\n", this->header[0],
+               this->header[1], this->header[2], this->app_id, this->ref_id, crc, this->random,
+               std::memcmp(&header, &transport_header, 3) == 0, (this->app_id != 0), (this->random == crc), valid);
+    }
+
+    this->random = crc;
+    return valid;
+  }
+
+  inline std::string to_str(uint64_t mac64_ = 0) {
+    espnow_addr_t mac;
+    if (mac64_ == 0)
+      mac64_ = this->mac64;
+    memcpy((void *) &mac, &mac64_, 6);
+    return string_format("{\"%02x:%02x:%02x:%02x:%02x:%02x\"}", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+  }
+
+  inline uint8_t *dataptr() { return (uint8_t *) &content; }
+};
+
+}  // namespace espnow
+}  // namespace esphome