ESP-NOW strategy support for esp8266

src/interfaces/ARDUINO/ESPNOWHelper.h

@@ -36,8 +36,7 @@
 	#endif
 #endif
 
-#define ESPNOW_PACKET_HEADER_LENGTH 2
-#define ESPNOW_PACKET_FOOTER_LENGTH 2
+
 
 
 static uint8_t espnow_broadcast_mac[ESP_NOW_MAC_LENGTH] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
@@ -60,22 +59,12 @@ static void espnow_send_cb(const uint8_t *mac_addr, uint8_t status) {
 };
 
 static void espnow_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len) {
-	espnow_packet_t* packet = new espnow_packet_t();
-
-	memcpy(packet->mac_addr, mac_addr, ESP_NOW_MAC_LENGTH);
-
-	packet->data = (uint8_t *)malloc(len);
-	memcpy(packet->data, data, len);
-	packet->data_len = len;
-
-	if (!packetQueue.push(packet)) { // queue is full - drop the packet
-		free(packet->data);
-		delete packet;
-	}
+    // Add the packet to the incoming queue.
+    // no error checking, if queue is full just ignore the packet.
+	packetQueue.push(mac_addr, data, len);
 };
 
 class ENHelper {
-	uint8_t _magic_header[4];
 	uint8_t _channel = 14;
 	uint8_t _esp_pmk[16];
 
@@ -151,47 +140,7 @@ class ENHelper {
 	};
 
 	uint16_t receive_frame(uint8_t *data, uint16_t max_length) {
-		espnow_packet_t* packet = packetQueue.pop(); // see if there's any received data waiting
-		if (packet == NULL)
-			return PJON_FAIL;
-
-		if (packet->data_len < 4) { // The packet is too small
-			Serial.println("too small");
-			free(packet->data);
-			delete(packet);
-			return PJON_FAIL;
-		}
-
-		uint8_t len = packet->data_len - 4;
-
-		if(
-			(packet->data[0] ^ len) != _magic_header[0] ||
-			(packet->data[1] ^ len) != _magic_header[1] ||
-			(packet->data[packet->data_len - 2] ^ len) != _magic_header[2] ||
-			(packet->data[packet->data_len - 1] ^ len) != _magic_header[3]
-		) {
-			Serial.println("Magic mismatch");
-			free(packet->data);
-			delete(packet);
-			return PJON_FAIL;
-		}
-
-		//Serial.print("H"); Serial.println(packet->data[2]);
-
-		if (len > max_length) {
-			free(packet->data);
-			delete(packet);
-			return PJON_FAIL;
-		}
-
-		memcpy(last_mac, packet->mac_addr, ESP_NOW_MAC_LENGTH);  // Update last received mac
-
-		memcpy(data, packet->data + 2, len);
-
-		free(packet->data);
-		delete(packet);
-
-		return len;
+		return packetQueue.pop(last_mac, data, max_length);
 	};
 
 	void send_frame(uint8_t *data, uint16_t length, uint8_t dest_mac[ESP_NOW_MAC_LENGTH]) {
@@ -208,11 +157,11 @@ class ENHelper {
 		uint8_t packet[PJON_PACKET_MAX_LENGTH + ESPNOW_PACKET_HEADER_LENGTH + ESPNOW_PACKET_FOOTER_LENGTH];
 
 		uint8_t len = length;
-		packet[0] = _magic_header[0] ^ len;
-		packet[1] = _magic_header[1] ^ len;
+		packet[0] = ESP_NOW_MAGIC_HEADER[0] ^ len;
+		packet[1] = ESP_NOW_MAGIC_HEADER[1] ^ len;
 		memcpy(packet + ESPNOW_PACKET_HEADER_LENGTH, data, len);
-		packet[len + 2] = _magic_header[2] ^ len;
-		packet[len + 3] = _magic_header[3] ^ len;
+		packet[len + 2] = ESP_NOW_MAGIC_HEADER[2] ^ len;
+		packet[len + 3] = ESP_NOW_MAGIC_HEADER[3] ^ len;
 
 		ATOMIC()
 		{
@@ -233,10 +182,6 @@ class ENHelper {
 		send_frame(data, length, espnow_broadcast_mac);
 	};
 
-	void set_magic_header(uint8_t *magic_header) {
-		memcpy(_magic_header, magic_header, 4);
-	};
-
 	void get_sender(uint8_t *ip) {
 		memcpy(ip, last_mac, ESP_NOW_MAC_LENGTH);
 	};

src/interfaces/ARDUINO/PacketQueue.h

@@ -1,25 +1,63 @@
 #pragma once
 
 #if defined(ESP8266)
-    #include <c_types.h>
-    #include <espnow.h>
+	#include <c_types.h>
+	#include <espnow.h>
 #elif defined(ESP32)
-    #include <esp_now.h>
+	#include <esp_now.h>
 #endif
 
 #include <SimplyAtomic.h> // https://github.com/wizard97/SimplyAtomic
 #include "PJONDefines.h"
 
-#ifndef ESP_NOW_MAC_LENGTH
-    #define ESP_NOW_MAC_LENGTH 6
-#endif
+#define ESP_NOW_MAC_LENGTH 6
+#define ESPNOW_PACKET_HEADER_LENGTH 2
+#define ESPNOW_PACKET_FOOTER_LENGTH 2
+uint8_t* const ESP_NOW_MAGIC_HEADER = (uint8_t*)"\xEE\xFE\x0E\xEF";
 
-typedef struct
+class EspNowPacket
 {
-    uint8_t mac_addr[ESP_NOW_MAC_LENGTH];
-    uint8_t *data;
-    int data_len;
-} espnow_packet_t;
+	private:
+		uint8_t mac_address[ESP_NOW_MAC_LENGTH];
+		uint8_t data[PJON_PACKET_MAX_LENGTH + ESPNOW_PACKET_HEADER_LENGTH + ESPNOW_PACKET_FOOTER_LENGTH];
+		int data_len;
+
+	public:
+		void set(const uint8_t *mac_address, const uint8_t *data, int len) {
+			memcpy(this->mac_address, mac_address, ESP_NOW_MAC_LENGTH);
+			memcpy(this->data, data, len);
+			this->data_len = len;
+		}
+
+		uint16_t checkAndGet(uint8_t *out_mac_address, uint8_t *out_data, uint16_t max_length) {
+			if (data_len < ESPNOW_PACKET_HEADER_LENGTH + ESPNOW_PACKET_FOOTER_LENGTH) {
+				// The packet is too small
+				return PJON_FAIL;
+			}
+
+			uint8_t len = data_len - ESPNOW_PACKET_HEADER_LENGTH - ESPNOW_PACKET_FOOTER_LENGTH;
+
+			if(
+				(data[0] ^ len) != ESP_NOW_MAGIC_HEADER[0] ||
+				(data[1] ^ len) != ESP_NOW_MAGIC_HEADER[1] ||
+				(data[data_len - 2] ^ len) != ESP_NOW_MAGIC_HEADER[2] ||
+				(data[data_len - 1] ^ len) != ESP_NOW_MAGIC_HEADER[3]
+			) {
+				// Magic mismatch
+				return PJON_FAIL;
+			}
+
+			if (len > max_length) {
+				// Too short buffer
+				return PJON_FAIL;
+			}
+
+			memcpy(out_mac_address, mac_address, ESP_NOW_MAC_LENGTH);
+			memcpy(out_data, data + ESPNOW_PACKET_HEADER_LENGTH, len);
+
+            return len;
+		}
+};
 
 /**
  * A queue of incoming packets of max capacity PJON_MAX_PACKETS.
@@ -28,58 +66,61 @@ typedef struct
  */
 class PacketQueue
 {
-    private:
-        // 1 extra capacity is necessary to distinguisch between the queue being full and empty
-        espnow_packet_t* queue[PJON_MAX_PACKETS + 1];
-        volatile uint8_t firstElement = 0;
-        volatile uint8_t firstSpace = 0;
-
-    public:
-        /**
-         * Adds a packet to the queue.
-         * @return true if successfull, false if the queue was full and the packet was not added.
-         */
-        bool push(espnow_packet_t* packet);
-
-        /**
-         * Remove the first packet from the queue and return it.
-         * @return first packet or NULL if the queue is empty
-         */
-        espnow_packet_t* pop();
+	private:
+		// 1 extra capacity is necessary to distinguisch between the queue being full and empty
+		EspNowPacket queue[PJON_MAX_PACKETS + 1];
+		volatile uint8_t firstElement = 0;
+		volatile uint8_t firstSpace = 0;
+
+	public:
+		/**
+		 * Adds a packet to the queue.
+		 * @return true if successfull, false if the queue was full and the packet was not added.
+		 */
+		bool push(const uint8_t *mac_addr, const uint8_t *data, int len);
+
+		/**
+		 * Remove the first packet from the queue, check it for length, remove ESPNOW headers and return it.
+		 * @param out_mac_address - pass in a buffer, the method will copy the sender's MAC address into it.
+		 * @param out_data - pass in an allocated buffer, the method will copy the data into it.
+		 * @param max_length - length of the allocated buffer.
+		 * @return length of the data or PJON_FAIL in case the queue is empty or there was an error
+		 */
+		uint16_t pop(uint8_t *out_mac_address, uint8_t *out_data, uint16_t max_length);
 };
 
-bool PacketQueue::push(espnow_packet_t* packet)
+bool PacketQueue::push(const uint8_t *mac_addr, const uint8_t *data, int len)
 {   
-    bool isFull;
-
-    ATOMIC()
-    {
-        int firstSpacePlus1 = (firstSpace + 1) % (PJON_MAX_PACKETS + 1);
-        isFull = firstSpacePlus1 == firstElement;
-        if (!isFull)
-        {
-            queue[firstSpace] = packet;
-            firstSpace = firstSpacePlus1;
-        }
-    }
-
-    return !isFull;
+	bool isFull;
+
+	ATOMIC()
+	{
+		int firstSpacePlus1 = (firstSpace + 1) % (PJON_MAX_PACKETS + 1);
+		isFull = firstSpacePlus1 == firstElement;
+		if (!isFull)
+		{
+			queue[firstSpace].set(mac_addr, data, len);
+			firstSpace = firstSpacePlus1;
+		}
+	}
+
+	return !isFull;
 }
 
-espnow_packet_t* PacketQueue::pop() {
-    if (firstElement == firstSpace) {
-        return NULL;
-    }
-    else
-    {
-        espnow_packet_t* packet;
-
-        ATOMIC()
-        {
-            packet = queue[firstElement];
-            firstElement = (firstElement + 1) % (PJON_MAX_PACKETS + 1);
-        }
-
-        return packet;
-    }
+uint16_t PacketQueue::pop(uint8_t *out_mac_address, uint8_t *out_data, uint16_t max_length) {
+	if (firstElement == firstSpace) {
+		return PJON_FAIL;
+	}
+	else
+	{
+		uint16_t length;
+
+		ATOMIC()
+		{
+			length = queue[firstElement].checkAndGet(out_mac_address, out_data, max_length);
+			firstElement = (firstElement + 1) % (PJON_MAX_PACKETS + 1);
+		}
+
+		return length;
+	}
 }

src/strategies/ESPNOW/ESPNOW.h

@@ -36,7 +36,7 @@
   #define EN_RECEIVE_TIME 0
 #endif
 
-#define EN_MAGIC_HEADER (uint8_t*)"\xEE\xFE\x0E\xEF"
+
 
 class ESPNOW {
     bool _espnow_initialised = false;
@@ -54,7 +54,6 @@ class ESPNOW {
 
     bool check_en() {
       if(!_espnow_initialised) {
-        en.set_magic_header(EN_MAGIC_HEADER);
         if(en.begin(_channel,(uint8_t*)_espnow_pmk))
           _espnow_initialised = true;
       }