text: #include <stdio.h>
  #include <stdlib.h>
  #include "freertos/FreeRTOS.h"
  #include "freertos/task.h"
  #include "freertos/queue.h"
  #include "freertos/semphr.h"
  #include "unity.h"
  
  /*
   * Basic queue set tests. Multiple queues are added to a queue set then each
   * queue is filled in a sequential order. The members returned from the queue
   * set must adhered to the order in which the queues were filled.
   */
  #define NO_OF_QUEUES            5
  #define QUEUE_LEN               4
  #define ITEM_SIZE               sizeof(uint32_t)
  
  static QueueHandle_t handles[NO_OF_QUEUES];
  static QueueSetHandle_t set_handle;
  
  TEST_CASE("Test Queue sets", "[freertos]")
  {
      //Create queue set, queues, and add queues to queue set
      set_handle = xQueueCreateSet(NO_OF_QUEUES * QUEUE_LEN);
      for (int i = 0; i < NO_OF_QUEUES; i++) {
          handles[i] = xQueueCreate(QUEUE_LEN, ITEM_SIZE);
          TEST_ASSERT_MESSAGE(handles[i] != NULL, "Failed to create queue");
          TEST_ASSERT_MESSAGE(xQueueAddToSet(handles[i], set_handle) == pdPASS, "Failed to add to queue set");
      }
  
      //Fill queue set via filling each queue
      for (int i = 0; i < NO_OF_QUEUES; i++) {
          for (int j = 0; j < QUEUE_LEN; j++) {
              uint32_t item_num = (i * QUEUE_LEN) + j;
              TEST_ASSERT_MESSAGE(xQueueSendToBack(handles[i], &item_num, portMAX_DELAY) == pdTRUE, "Failed to send to queue");
          }
      }
  
      //Check queue set is notified in correct order
      for (int i = 0; i < NO_OF_QUEUES; i++) {
          for (int j = 0; j < QUEUE_LEN; j++) {
              QueueSetMemberHandle_t member = xQueueSelectFromSet(set_handle, portMAX_DELAY);
              TEST_ASSERT_EQUAL_MESSAGE(handles[i], member, "Incorrect queue set member returned");
              uint32_t item;
              xQueueReceive((QueueHandle_t)member, &item, 0);
              TEST_ASSERT_EQUAL_MESSAGE(((i * QUEUE_LEN) + j), item, "Incorrect item value");
          }
      }
  
      //Remove queues from queue set and delete queues
      for (int i = 0; i < NO_OF_QUEUES; i++) {
          TEST_ASSERT_MESSAGE(xQueueRemoveFromSet(handles[i], set_handle), "Failed to remove from queue set");
          vQueueDelete(handles[i]);
      }
      vQueueDelete(set_handle);
  }
  
  /*
   * Queue set thread safety test. Test the SMP thread safety by adding two queues
   * to a queue set and have a task on each core send to the queues simultaneously.
   * Check returned queue set members are valid.
   */
  #ifndef CONFIG_FREERTOS_UNICORE
  static volatile bool sync_flags[portNUM_PROCESSORS];
  static SemaphoreHandle_t sync_sem;
  
  static void send_task(void *arg)
  {
      QueueHandle_t queue = (QueueHandle_t)arg;
  
      //Wait until task on the other core starts running
      xSemaphoreTake(sync_sem, portMAX_DELAY);
      sync_flags[xPortGetCoreID()] = true;
      while (!sync_flags[!xPortGetCoreID()]) {
          ;
      }
  
      //Fill queue
      for (int i = 0; i < QUEUE_LEN; i++) {
          uint32_t item = i;
          xQueueSendToBack(queue, &item, portMAX_DELAY);
      }
  
      xSemaphoreGive(sync_sem);
      vTaskDelete(NULL);
  }
  
  TEST_CASE("Test Queue sets thread safety", "[freertos]")
  {
      //Create queue set, queues, and a send task on each core
      sync_sem = xSemaphoreCreateCounting(portNUM_PROCESSORS, 0);
      QueueHandle_t queue_handles[portNUM_PROCESSORS];
      QueueSetHandle_t queueset_handle = xQueueCreateSet(portNUM_PROCESSORS * QUEUE_LEN);
      for (int i = 0; i < portNUM_PROCESSORS; i++) {
          sync_flags[i] = false;
          queue_handles[i] = xQueueCreate(QUEUE_LEN, ITEM_SIZE);
          TEST_ASSERT_MESSAGE(xQueueAddToSet(queue_handles[i], queueset_handle) == pdPASS, "Failed to add to queue set");
          xTaskCreatePinnedToCore(send_task, "send", 2048, (void *)queue_handles[i], 10, NULL, i);
      }
  
      //Start both send tasks
      portDISABLE_INTERRUPTS();
      for (int i = 0; i < portNUM_PROCESSORS; i++) {
          xSemaphoreGive(sync_sem);
      }
      portENABLE_INTERRUPTS();
      vTaskDelay(2);
  
      //Check returned queue set members are valid
      uint32_t expect_0 = 0;
      uint32_t expect_1 = 0;
      for (int i = 0; i < (portNUM_PROCESSORS * QUEUE_LEN); i++) {
          QueueSetMemberHandle_t member = xQueueSelectFromSet(queueset_handle, portMAX_DELAY);
          uint32_t item;
          if (member == queue_handles[0]) {
              xQueueReceive((QueueHandle_t)member, &item, 0);
              TEST_ASSERT_EQUAL_MESSAGE(expect_0, item, "Incorrect item value");
              expect_0++;
          } else if (member == queue_handles[1]) {
              xQueueReceive((QueueHandle_t)member, &item, 0);
              TEST_ASSERT_EQUAL_MESSAGE(expect_1, item, "Incorrect item value");
              expect_1++;
          } else {
              TEST_ASSERT_MESSAGE(0, "Incorrect queue set member returned");
          }
      }
  
      for (int i = 0; i < portNUM_PROCESSORS; i++) {
          xSemaphoreTake(sync_sem, portMAX_DELAY);
      }
      for (int i = 0; i < portNUM_PROCESSORS; i++) {
          xQueueRemoveFromSet(queueset_handle, handles[i]);
          vQueueDelete(queue_handles[i]);
      }
      vQueueDelete(queueset_handle);
  }
  #endif
  