EspNow.cpp

/**
 * @file EspNow.cpp
 * @author Phil Hilger (phil@peergum.com)
 * @brief 
 * @version 0.1
 * @date 2023-03-02
 * 
 * CAN-talk. A library for microcontrollers that allows decent comms
 * over a CAN bus.
 * 
 * Copyright (C) 2023, PeerGum
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https: //www.gnu.org/licenses/>.
 * 
 */

#include "EspNow.h"
#include <cstring>
#include "esp_log.h"
#include "esp_random.h"
#include "esp_mac.h"
#include "esp_crc.h"

static const char *TAG = "ESP-Now";

EspNow *EspNow::_instance = NULL;

uint8_t EspNow::s_broadcast_mac[ESP_NOW_ETH_ALEN] = {0xFF, 0xFF, 0xFF,
                                                     0xFF, 0xFF, 0xFF};
uint16_t EspNow::s_espnow_seq[ESPNOW_DATA_MAX] = {0, 0};

EspNow::EspNow() {
}

EspNow::~EspNow() {

}

/* ESPNOW sending or receiving callback function is called in WiFi task.
 * Users should not do lengthy operations from this task. Instead, post
 * necessary data to a queue and handle it from a lower priority task. */
void EspNow::sendCallback(const uint8_t *mac_addr,
                          esp_now_send_status_t status) {
  espnow_event_t evt;
  espnow_event_send_cb_t *send_cb = &evt.info.send_cb;

  if (mac_addr == NULL) {
    ESP_LOGE(TAG, "Send cb arg error");
    return;
  }

  evt.id = ESPNOW_SEND_CB;
  memcpy(send_cb->mac_addr, mac_addr, ESP_NOW_ETH_ALEN);
  send_cb->status = status;
  if (xQueueSend(_instance->s_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) {
    ESP_LOGW(TAG, "Send send queue fail");
  }
}

void EspNow::receiveCallback(const uint8_t *mac_addr, const uint8_t *data,
                     int len) {
  espnow_event_t evt;
  espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb;

  if (mac_addr == NULL || data == NULL || len <= 0) {
    ESP_LOGE(TAG, "Receive cb arg error");
    return;
  }

  evt.id = ESPNOW_RECV_CB;
  memcpy(recv_cb->mac_addr, mac_addr, ESP_NOW_ETH_ALEN);
  recv_cb->data = (uint8_t *)malloc(len);
  if (recv_cb->data == NULL) {
    ESP_LOGE(TAG, "Malloc receive data fail");
    return;
  }
  memcpy(recv_cb->data, data, len);
  recv_cb->data_len = len;
  if (xQueueSend(_instance->s_queue, &evt, ESPNOW_MAXDELAY) != pdTRUE) {
    ESP_LOGW(TAG, "Send receive queue fail");
  }
}

/* Parse received ESPNOW data. */
int EspNow::data_parse(uint8_t *data, uint16_t data_len, uint8_t *state,
                            uint16_t *seq, int *magic) {
  espnow_data_t *buf = (espnow_data_t *)data;
  uint16_t crc, crc_cal = 0;

  if (data_len < sizeof(espnow_data_t)) {
    ESP_LOGE(TAG, "Receive ESPNOW data too short, len:%d", data_len);
    return -1;
  }

  *state = buf->state;
  *seq = buf->seq_num;
  *magic = buf->magic;
  crc = buf->crc;
  buf->crc = 0;
  crc_cal = esp_crc16_le(UINT16_MAX, (uint8_t const *)buf, data_len);

  if (buf->ssid[0]) {
    ESP_LOGI(TAG, "SSID [%s], password [%s]", buf->ssid, buf->password);
  } else {
    ESP_LOGI(TAG, "Empty payload");
  }

  // if (!strcmp(buf->payload,REMOTE_USB_ESPNOW_ID)) {
  //   *state = 2; // proper device
  // }
  if (crc_cal == crc) {
    return buf->type;
  }

  return -1;
}

/* Prepare ESPNOW data to be sent. */
void EspNow::data_prepare(espnow_send_param_t *send_param) {
  espnow_data_t *buf = (espnow_data_t *)send_param->buffer;

  assert(send_param->len >= sizeof(espnow_data_t));

  buf->type = IS_BROADCAST_ADDR(send_param->dest_mac) ? ESPNOW_DATA_BROADCAST
                                                      : ESPNOW_DATA_UNICAST;
  buf->state = send_param->state;
  buf->seq_num = s_espnow_seq[buf->type]++;
  buf->crc = 0;
  buf->magic = send_param->magic;
  /* Fill all remaining bytes after the data with random values */
  if (buf->type == ESPNOW_DATA_BROADCAST) {
    buf->ssid[0] = 0;
    buf->password[0] = 0;
  } else {
    // strcpy(buf->payload, REMOTE_USB_ESPNOW_ID);
    strcpy((char *)buf->ssid, _instance->config->getStringByName("ssid"));
    strcpy((char *)buf->password,
           _instance->config->getStringByName("password"));
  }
  buf->crc = esp_crc16_le(UINT16_MAX, (uint8_t const *)buf, send_param->len);
}

void EspNow::task(void *pvParameter) {
  espnow_event_t evt;
  uint8_t recv_state = 0;
  uint16_t recv_seq = 0;
  int recv_magic = 0;
  bool is_broadcast = false;
  int ret;

  vTaskDelay(5000 / portTICK_PERIOD_MS);
  ESP_LOGI(TAG, "Start sending broadcast data");

  /* Start sending broadcast ESPNOW data. */
  espnow_send_param_t *send_param = (espnow_send_param_t *)pvParameter;
  esp_err_t err;
  err = esp_now_send(send_param->dest_mac, send_param->buffer, send_param->len);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "Initial Send error: %x", err);
    deinit(send_param);
    vTaskDelete(NULL);
    return;
  }

  while (xQueueReceive(_instance->s_queue, &evt, portMAX_DELAY) == pdTRUE) {
    switch (evt.id) {
    case ESPNOW_SEND_CB: {
      espnow_event_send_cb_t *send_cb = &evt.info.send_cb;
      is_broadcast = IS_BROADCAST_ADDR(send_cb->mac_addr);

      ESP_LOGD(TAG, "Send data to " MACSTR ", status1: %d",
               MAC2STR(send_cb->mac_addr), send_cb->status);

      if (is_broadcast && (send_param->broadcast == false)) {
        break;
      }

      if (!is_broadcast) {
        send_param->count--;
        if (send_param->count == 0) {
          ESP_LOGI(TAG, "Send done");
          deinit(send_param);
          vTaskDelete(NULL);
        }
      }

      /* Delay a while before sending the next data. */
      if (send_param->delay > 0) {
        vTaskDelay(send_param->delay / portTICK_PERIOD_MS);
      }

      memcpy(send_param->dest_mac, send_cb->mac_addr, ESP_NOW_ETH_ALEN);
      data_prepare(send_param);

      ESP_LOGI(TAG, "send data to " MACSTR "", MAC2STR(send_cb->mac_addr));

      /* Send the next data after the previous data is sent. */
      if (esp_now_send(send_param->dest_mac, send_param->buffer,
                       send_param->len) != ESP_OK) {
        ESP_LOGE(TAG, "Send error");
        deinit(send_param);
        vTaskDelete(NULL);
      }
      break;
    }
    case ESPNOW_RECV_CB: {
      espnow_event_recv_cb_t *recv_cb = &evt.info.recv_cb;

      ret = data_parse(recv_cb->data, recv_cb->data_len, &recv_state,
                              &recv_seq, &recv_magic);
      free(recv_cb->data);
      if (ret == ESPNOW_DATA_BROADCAST) {
        ESP_LOGI(TAG, "Receive %dth broadcast data from: " MACSTR ", len: %d",
                 recv_seq, MAC2STR(recv_cb->mac_addr), recv_cb->data_len);
        /* If MAC address does not exist in peer list, add it to peer list. */
        if (esp_now_is_peer_exist(recv_cb->mac_addr) == false) {
          esp_now_peer_info_t *peer =
              (esp_now_peer_info_t *)malloc(sizeof(esp_now_peer_info_t));
          if (peer == NULL) {
            ESP_LOGE(TAG, "Malloc peer information fail");
            deinit(send_param);
            vTaskDelete(NULL);
          }
          memset(peer, 0, sizeof(esp_now_peer_info_t));
          peer->channel = 6; //CONFIG_ESPNOW_CHANNEL;
          peer->ifidx = ESPNOW_WIFI_IF;
          peer->encrypt = true;
          memcpy(peer->lmk, CONFIG_ESPNOW_LMK, ESP_NOW_KEY_LEN);
          memcpy(peer->peer_addr, recv_cb->mac_addr, ESP_NOW_ETH_ALEN);
          ESP_ERROR_CHECK(esp_now_add_peer(peer));
          free(peer);
        }

        /* Indicates that the device has received broadcast ESPNOW data. */
        if (send_param->state == 0) {
          send_param->state = 1;
        }

        /* If receive broadcast ESPNOW data which indicates that the other
         * device has received broadcast ESPNOW data and the local magic
         * number is bigger than that in the received broadcast ESPNOW data,
         * stop sending broadcast ESPNOW data and start sending unicast ESPNOW
         * data.
         */
        if (recv_state == 1) {
          /* The device which has the bigger magic number sends ESPNOW data,
           * the other one receives ESPNOW data.
           */
          if (send_param->unicast == false && send_param->magic >= recv_magic) {
            ESP_LOGI(TAG, "Start sending unicast data");
            ESP_LOGI(TAG, "send data to " MACSTR "",
                     MAC2STR(recv_cb->mac_addr));

            /* Start sending unicast ESPNOW data. */
            memcpy(send_param->dest_mac, recv_cb->mac_addr, ESP_NOW_ETH_ALEN);
            data_prepare(send_param);
            if (esp_now_send(send_param->dest_mac, send_param->buffer,
                             send_param->len) != ESP_OK) {
              ESP_LOGE(TAG, "Send error");
              deinit(send_param);
              vTaskDelete(NULL);
            } else {
              send_param->broadcast = false;
              send_param->unicast = true;
            }
          }
        }
      } else if (ret == ESPNOW_DATA_UNICAST) {
        ESP_LOGI(TAG, "Receive %dth unicast data from: " MACSTR ", len: %d",
                 recv_seq, MAC2STR(recv_cb->mac_addr), recv_cb->data_len);

        /* If receive unicast ESPNOW data, also stop sending broadcast ESPNOW
         * data. */
        send_param->broadcast = false;
      } else {
        ESP_LOGI(TAG, "Receive error data from: " MACSTR "",
                 MAC2STR(recv_cb->mac_addr));
        // ESP_LOG_BUFFER_HEXDUMP(TAG, recv_cb->data, recv_cb->data_len,
        //  ESP_LOG_INFO);
      }
      break;
    }
    default:
      ESP_LOGE(TAG, "Callback type error: %d", evt.id);
      break;
    }
  }
}

esp_err_t EspNow::init(Config *_config) {
  if (!_instance) {
    _instance = new EspNow();
  }

  _instance->config = _config;
  espnow_send_param_t *send_param;

  _instance->s_queue = xQueueCreate(ESPNOW_QUEUE_SIZE, sizeof(espnow_event_t));
  if (_instance->s_queue == NULL) {
    ESP_LOGE(TAG, "Create mutex fail");
    return ESP_FAIL;
  }

  /* Initialize ESPNOW and register sending and receiving callback function. */
  ESP_ERROR_CHECK(esp_now_init());
  ESP_ERROR_CHECK(esp_now_register_send_cb(_instance->sendCallback));
  ESP_ERROR_CHECK(esp_now_register_recv_cb(_instance->receiveCallback));
#if CONFIG_ESP_WIFI_STA_DISCONNECTED_PM_ENABLE
  ESP_ERROR_CHECK(esp_now_set_wake_window(65535));
#endif
  /* Set primary master key. */
  ESP_ERROR_CHECK(esp_now_set_pmk((uint8_t *)CONFIG_ESPNOW_PMK));

  /* Add broadcast peer information to peer list. */
  esp_now_peer_info_t *peer =
      (esp_now_peer_info_t *)malloc(sizeof(esp_now_peer_info_t));
  if (peer == NULL) {
    ESP_LOGE(TAG, "Malloc peer information fail");
    vSemaphoreDelete(_instance->s_queue);
    esp_now_deinit();
    return ESP_FAIL;
  }
  memset(peer, 0, sizeof(esp_now_peer_info_t));
  peer->channel = 0;//CONFIG_ESPNOW_CHANNEL;
  peer->ifidx = ESPNOW_WIFI_IF;
  peer->encrypt = false;
  memcpy(peer->peer_addr, s_broadcast_mac, ESP_NOW_ETH_ALEN);
  ESP_ERROR_CHECK(esp_now_add_peer(peer));
  free(peer);

  /* Initialize sending parameters. */
  send_param = (espnow_send_param_t *)malloc(sizeof(espnow_send_param_t));
  if (send_param == NULL) {
    ESP_LOGE(TAG, "Malloc send parameter fail");
    vSemaphoreDelete(_instance->s_queue);
    esp_now_deinit();
    return ESP_FAIL;
  }
  memset(send_param, 0, sizeof(espnow_send_param_t));
  send_param->unicast = false;
  send_param->broadcast = true;
  send_param->state = 0;
  send_param->magic = esp_random();
  send_param->count = CONFIG_ESPNOW_SEND_COUNT;
  send_param->delay = CONFIG_ESPNOW_SEND_DELAY;
  send_param->len = CONFIG_ESPNOW_SEND_LEN;
  send_param->buffer = (uint8_t *)malloc(CONFIG_ESPNOW_SEND_LEN);
  if (send_param->buffer == NULL) {
    ESP_LOGE(TAG, "Malloc send buffer fail");
    free(send_param);
    vSemaphoreDelete(_instance->s_queue);
    esp_now_deinit();
    return ESP_FAIL;
  }
  memcpy(send_param->dest_mac, s_broadcast_mac, ESP_NOW_ETH_ALEN);
  data_prepare(send_param);

  xTaskCreate(_instance->task, "espnow_task", 4096, send_param, 4, NULL);

  return ESP_OK;
}

void EspNow::deinit(espnow_send_param_t *send_param) {
  free(send_param->buffer);
  free(send_param);
  vSemaphoreDelete(_instance->s_queue);
  esp_now_deinit();
}

EspNow &EspNow::instance() { return *_instance; }