esp_alox/main/communication_handler.c

#include "communication_handler.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_now.h"
#include "esp_ota_ops.h"
#include "esp_partition.h"
#include "esp_timer.h"
#include "freertos/idf_additions.h"
#include "freertos/task.h"
#include "message_structs.h"
#include "ota_update.h"

#include "client_handler.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>

uint8_t broadcast_address[ESP_NOW_ETH_ALEN] = {0xFF, 0xFF, 0xFF,
                                               0xFF, 0xFF, 0xFF};

static const char *TAG = "ALOX - COM";

static QueueHandle_t messageQueue = NULL;
static struct ESP_MessageBroker mr;
static QueueHandle_t ESP_recieved_message_queue;

void free_ESPNOW_MessageInfo(ESPNOW_MessageInfo *msg) {
  if (msg->esp_now_info.src_addr) {
    free(msg->esp_now_info.src_addr);
    msg->esp_now_info.src_addr = NULL;
  }
  if (msg->esp_now_info.des_addr) {
    free(msg->esp_now_info.des_addr);
    msg->esp_now_info.des_addr = NULL;
  }
  if (msg->esp_now_info.rx_ctrl) {
    free(msg->esp_now_info.rx_ctrl);
    msg->esp_now_info.rx_ctrl = NULL;
  }
  if (msg->data) {
    free(msg->data);
    msg->data = NULL;
  }
}

void ESP_InitMessageBroker(QueueHandle_t msg_queue_handle) {
  mr.num_direct_callbacks = 0;
  mr.num_task_callbacks = 0;
  ESP_recieved_message_queue = msg_queue_handle;
  return;
}

void ESP_RegisterFunction(CommandPages command,
                          ESP_RegisterFunctionCallback callback) {
  mr.FunctionList[mr.num_direct_callbacks].MSGID = command;
  mr.FunctionList[mr.num_direct_callbacks].callback = callback;
  mr.num_direct_callbacks++;
  return;
}

void ESP_RegisterTask(CommandPages command, ESP_RegisterTaskCallback callback) {
  mr.TaskList[mr.num_task_callbacks].MSGID = command;
  mr.TaskList[mr.num_task_callbacks].task = callback;
  mr.num_task_callbacks++;
}

void ESP_MessageBrokerTask(void *param) {
  ESPNOW_MessageInfo received_msg;
  ESP_MessageBrokerTaskParams_t *task_params =
      (ESP_MessageBrokerTaskParams_t *)param;

  QueueHandle_t msg_queue = task_params->message_queue;

  if (msg_queue == NULL) {
    ESP_LOGE(TAG, "Message queue not initialized. Terminating task.");
    vTaskDelete(NULL);
  }

  ESP_LOGI(TAG, "Message broker task started.");

  while (1) {
    if (xQueueReceive(msg_queue, &received_msg, portMAX_DELAY)) {

      const BaseMessage *message = (const BaseMessage *)received_msg.data;
      for (int i = 0; i < mr.num_direct_callbacks; i++) {
        if (mr.FunctionList[i].MSGID == message->commandPage) {
          mr.FunctionList[i].callback(&received_msg.esp_now_info,
                                      received_msg.data, received_msg.data_len);
          free_ESPNOW_MessageInfo(&received_msg);
        }
      }
    }
  }
}

static bool hasMaster = false;
static ClientList *esp_client_list;
static uint8_t channelNumber = 0;

int init_com(ClientList *clients, uint8_t wifi_channel) {
  messageQueue = xQueueCreate(MESSAGE_QUEUE_SIZE, sizeof(ESPNOW_MessageInfo));
  if (messageQueue == NULL) {
    ESP_LOGE(TAG, "Message queue creation failed");
    return -1;
  }

  esp_client_list = clients;
  hasMaster = false;
  channelNumber = wifi_channel;
  return 0;
}

int add_peer(uint8_t *macAddr) {
  esp_now_peer_info_t peerInfo = {
      .channel = channelNumber,
      .ifidx = ESP_IF_WIFI_STA,
      .encrypt = false,
  };
  memcpy(peerInfo.peer_addr, macAddr, ESP_NOW_ETH_ALEN);

  esp_err_t result = esp_now_add_peer(&peerInfo);
  if (result == ESP_OK) {
    ESP_LOGI(TAG, "Peer added");

    if (!IS_BROADCAST_ADDR(macAddr)) {
      int ret = add_client(esp_client_list, peerInfo.peer_addr);
      if (ret < 0) {
        ESP_LOGE(TAG, "Client could not be added to client handler, removing "
                      "it from esp now client list!");
        esp_now_del_peer(peerInfo.peer_addr);
        return -1;
      }
      ESP_LOGI(TAG, "New client added.");
    }
  } else if (result == ESP_ERR_ESPNOW_EXIST) {
    ESP_LOGW(TAG, "Peer already exists.");
    int id = get_client_id(esp_client_list, peerInfo.peer_addr);
    if (id >= 0) {
      esp_client_list->Clients[id].isAvailable = true;
    }
  } else {
    ESP_LOGE(TAG, "Failed to add peer: %s", esp_err_to_name(result));
    return -1;
  }
  return 0;
}

BaseMessage MessageBuilder(CommandPages commandPage, PayloadUnion payload,
                           size_t payload_size) {
  BaseMessage message;

  message.commandPage = commandPage;
  message.version = 1;
  message.length = (uint16_t)payload_size;

  memset(&message.payload, 0, sizeof(message.payload));
  memcpy(&message.payload, &payload, payload_size);

  return message;
}

void master_broadcast_task(void *param) {
  while (1) {
    BroadCastPayload payload = {};

    BaseMessage message = MessageBuilder(
        BroadCastPage, *(PayloadUnion *)&payload, sizeof(payload));

    ESP_ERROR_CHECK(esp_now_send(broadcast_address, (uint8_t *)&message,
                                 sizeof(BaseMessage)));
    vTaskDelay(pdMS_TO_TICKS(5000));
  }
}

void master_broadcast_ping(void *param) {
  while (1) {
    PingPayload payload = {};
    payload.timestamp = esp_timer_get_time();
    BaseMessage message =
        MessageBuilder(PingPage, *(PayloadUnion *)&payload, sizeof(payload));
    ESP_ERROR_CHECK(esp_now_send(broadcast_address, (uint8_t *)&message,
                                 sizeof(BaseMessage)));
    vTaskDelay(pdMS_TO_TICKS(2500));
  }
}

void master_ping_task(void *param) {
  while (1) {
    for (size_t i = 0; i < MAX_CLIENTS; i++) {
      if (esp_client_list->Clients[i].isAvailable) {
        PingPayload payload = {};
        payload.timestamp = esp_timer_get_time();
        BaseMessage message = MessageBuilder(
            PingPage, *(PayloadUnion *)&payload, sizeof(payload));
        esp_now_send(esp_client_list->Clients[i].macAddr, (uint8_t *)&message,
                     sizeof(BaseMessage));
      }
    }
    vTaskDelay(pdMS_TO_TICKS(1000));
  }
}

void master_StatusCallback(const esp_now_recv_info_t *esp_now_info,
                           const uint8_t *data, int data_len) {
  const BaseMessage *message = (const BaseMessage *)data;
  ESP_LOGI(TAG, "SRC");
  ESP_LOGI(TAG,
           "Status Message Received: status: %d, runningPartition: %d, uptime: "
           "%d, version: %d",
           message->payload.status_payload.status,
           message->payload.status_payload.runningPartition,
           message->payload.status_payload.uptime,
           message->payload.status_payload.version);
}

void master_RegisterCallback(const esp_now_recv_info_t *esp_now_info,
                             const uint8_t *data, int data_len) {
  BaseMessage replyMessage = {};

  esp_now_peer_info_t checkPeerInfo;
  esp_err_t checkPeer =
      esp_now_get_peer(esp_now_info->src_addr, &checkPeerInfo);
  switch (checkPeer) {
  case (ESP_OK):
    int id = get_client_id(esp_client_list, esp_now_info->src_addr);
    esp_client_list->Clients[id].isAvailable = true;
    esp_client_list->Clients[id].lastSuccessfullPing = xTaskGetTickCount();
    break;
  case (ESP_ERR_ESPNOW_NOT_FOUND):
    add_peer(esp_now_info->src_addr);
    GetStatusPayload payload = {};
    replyMessage = MessageBuilder(GetStatusPage, *(PayloadUnion *)&payload,
                                  sizeof(payload));
    esp_now_send(esp_now_info->src_addr, (uint8_t *)&replyMessage,
                 sizeof(BaseMessage));
    break;
  default:
    break;
  }
}

void master_pingCallback(const esp_now_recv_info_t *esp_now_info,
                         const uint8_t *data, int data_len) {
  const BaseMessage *message = (const BaseMessage *)data;

  uint32_t currentTime = esp_timer_get_time();
  uint32_t diff = currentTime - message->payload.ping_payload.timestamp;

  int id = get_client_id(esp_client_list, esp_now_info->src_addr);
  if (id >= 0) {
    esp_client_list->Clients[id].lastSuccessfullPing = xTaskGetTickCount();
    esp_client_list->Clients[id].lastPing = (diff / 1000);
  }
}

void master_broadcastCallback(const esp_now_recv_info_t *esp_now_info,
                              const uint8_t *data, int data_len) {}

void ESPNOW_RegisterMasterCallbacks() {
  ESP_RegisterFunction(StatusPage, master_StatusCallback);
  ESP_RegisterFunction(RegisterPage, master_RegisterCallback);
  ESP_RegisterFunction(PingPage, master_pingCallback);
  ESP_RegisterFunction(BroadCastPage, master_broadcastCallback);
}

void slave_broadcastCallback(const esp_now_recv_info_t *esp_now_info,
                             const uint8_t *data, int data_len) {
  if (!hasMaster) {
    if (IS_BROADCAST_ADDR(esp_now_info->des_addr)) {
      add_peer(esp_now_info->src_addr);
      BaseMessage replyMessage = {};
      replyMessage =
          MessageBuilder(RegisterPage, *(PayloadUnion *)&replyMessage.payload,
                         sizeof(replyMessage.payload));
      esp_now_send(esp_now_info->src_addr, (uint8_t *)&replyMessage,
                   sizeof(BaseMessage));
      hasMaster = true;
    }
  }
}

void slave_getstatusCallback(const esp_now_recv_info_t *esp_now_info,
                             const uint8_t *data, int data_len) {
  StatusPayload payload = {
      .status = 1,
      .runningPartition = 1,
      .uptime = 100,
      .version = 0x0002,

  };
  BaseMessage replyMessage =
      MessageBuilder(StatusPage, *(PayloadUnion *)&payload, sizeof(payload));
  esp_now_send(esp_now_info->src_addr, (uint8_t *)&replyMessage,
               sizeof(BaseMessage));
}

void slave_pingCallback(const esp_now_recv_info_t *esp_now_info,
                        const uint8_t *data, int data_len) {
  if (!hasMaster)
    return;

  const BaseMessage *message = (const BaseMessage *)data;
  BaseMessage replyMessage = MessageBuilder(
      PingPage, *(PayloadUnion *)&message->payload, sizeof(message->payload));
  esp_now_send(esp_now_info->src_addr, (uint8_t *)&replyMessage,
               sizeof(BaseMessage));
}

void ESPNOW_RegisterSlaveCallbacks() {
  ESP_RegisterFunction(BroadCastPage, slave_broadcastCallback);
  ESP_RegisterFunction(GetStatusPage, slave_getstatusCallback);
  ESP_RegisterFunction(PingPage, slave_pingCallback);
}

void master_receive_callback(const esp_now_recv_info_t *esp_now_info,
                             const uint8_t *data, int data_len) {
  uint8_t *copied_data = (uint8_t *)malloc(data_len);
  if (copied_data == NULL) {
    return;
  }
  memcpy(copied_data, data, data_len);

  ESPNOW_MessageInfo msg_info;
  msg_info.esp_now_info.src_addr = malloc(6);
  if (msg_info.esp_now_info.src_addr) {
    memcpy(msg_info.esp_now_info.src_addr, esp_now_info->src_addr, 6);
  }

  msg_info.esp_now_info.des_addr = malloc(6);
  if (msg_info.esp_now_info.des_addr) {
    memcpy(msg_info.esp_now_info.des_addr, esp_now_info->des_addr, 6);
  }

  msg_info.esp_now_info.rx_ctrl = malloc(sizeof(wifi_pkt_rx_ctrl_t));
  if (msg_info.esp_now_info.rx_ctrl) {
    memcpy(msg_info.esp_now_info.rx_ctrl, esp_now_info->rx_ctrl,
           sizeof(wifi_pkt_rx_ctrl_t));
  }

  msg_info.data = copied_data;
  msg_info.data_len = data_len;

  xQueueSend(ESP_recieved_message_queue, &msg_info, portMAX_DELAY);
}

void client_receive_callback(const esp_now_recv_info_t *esp_now_info,
                             const uint8_t *data, int data_len) {
  uint8_t *copied_data = (uint8_t *)malloc(data_len);
  if (copied_data == NULL) {
    return;
  }
  memcpy(copied_data, data, data_len);

  ESPNOW_MessageInfo msg_info;
  // Initialize the esp_now_info struct to zeros
  memset(&msg_info.esp_now_info, 0, sizeof(esp_now_recv_info_t));

  // Now, allocate and copy the data pointed to by the pointers within
  // esp_now_info src_addr
  msg_info.esp_now_info.src_addr = malloc(6);
  if (msg_info.esp_now_info.src_addr) {
    memcpy(msg_info.esp_now_info.src_addr, esp_now_info->src_addr, 6);
  } else {
    free(copied_data);
    return;
  }

  // des_addr
  msg_info.esp_now_info.des_addr = malloc(6);
  if (msg_info.esp_now_info.des_addr) {
    memcpy(msg_info.esp_now_info.des_addr, esp_now_info->des_addr, 6);
  } else {
    free(msg_info.esp_now_info.src_addr);
    free(copied_data);
    return;
  }

  msg_info.esp_now_info.rx_ctrl = NULL; // Set to NULL as we are not copying it

  msg_info.data = copied_data;
  msg_info.data_len = data_len;

  xQueueSend(ESP_recieved_message_queue, &msg_info, portMAX_DELAY);
}

void client_monitor_task(void *pvParameters) {
  TickType_t timeout_ticks = pdMS_TO_TICKS(CLIENT_TIMEOUT_MS);
  TickType_t interval_ticks = pdMS_TO_TICKS(CHECK_INTERVAL_MS);

  while (1) {
    TickType_t now = xTaskGetTickCount();

    for (int i = 0; i < MAX_CLIENTS; i++) {
      if (esp_client_list->Clients[i].isAvailable) {
        TickType_t time_diff =
            now - esp_client_list->Clients[i].lastSuccessfullPing;

        if (time_diff > timeout_ticks) {
          esp_client_list->Clients[i].isAvailable = false;
        }
      }
    }
    vTaskDelay(interval_ticks);
  }
}

const esp_partition_t *ota_update_partition = NULL;

void ESPNOW_RegisterOTAMaster() {
  // Observe this States for all Slaves in ClientList
  // OTA_SLAVE_PREPARING
  // OTA_SLAVE_READY
  // OTA_SLAVE_ERROR
  // OTA_SLAVE_WRITE_FINISHED
  // OTA_SLAVE_FINISHED

  ESP_RegisterFunction(OTA_PREPARE_ACKNOWLEDGED,
                       master_ota_prepare_acknowledge_callback);
  ESP_RegisterFunction(OTA_READY_TO_RECEIVE,
                       master_ota_ready_to_recieve_callback);
  ESP_RegisterFunction(OTA_UPDATE_SLAVE_ACKED,
                       master_ota_update_slave_acknowledge_callback);
}

void ESPNOW_RegisterOTASlave() {
  ESP_RegisterFunction(OTA_PREPARE_FOR_UPDATE, slave_Prep_Upgrade_Callback);
  ESP_RegisterFunction(OTA_CHUNK, slave_Update_Chunk_Callback);

  ESP_RegisterFunction(OTA_FINISH_UPDATE, slave_Update_Finished_Callback);
}