source: ReferenceDesigns/w3_802.11/c/wlan_mac_high_framework/wlan_mac_queue.c

/** @file wlan_mac_queue.c
 *  @brief Queue Framework
 *
 *  This contains code for accessing the packet queue.
 *
 *  @copyright Copyright 2013-2019, Mango Communications. All rights reserved.
 *          Distributed under the Mango Communications Reference Design License
 *              See LICENSE.txt included in the design archive or
 *              at http://mangocomm.com/802.11/license
 *
 *  This file is part of the Mango 802.11 Reference Design (https://mangocomm.com/802.11)
 */

/***************************** Include Files *********************************/

#include "wlan_mac_high_sw_config.h"

// Xilinx / Standard library includes
#include "xil_types.h"
#include "stdlib.h"
#include "stdio.h"
#include "wlan_platform_high.h"
#include "string.h"

// WLAN includes
#include "wlan_mac_common.h"
#include "wlan_mac_high.h"
#include "wlan_mac_queue.h"
#include "wlan_mac_dl_list.h"
#include "wlan_mac_eth_util.h"
#include "wlan_mac_pkt_buf_util.h"
#include "wlan_platform_common.h"
#include "wlan_mac_station_info.h"

// WLAN Exp includes
#include "wlan_exp_common.h"

#define ADDRESS_SAFETY_CHECKS 0


/************************** Locally Scoped Types *************************************/
typedef struct _queue_t {
    dl_list list;
    u32 flags;
    function_ptr_t queue_state_change_callback;
    u32 callback_arg;
    u32 max_len;
} _queue_t;

#define _QUEUE_FLAGS_OPEN 0x00000001

/********************** Externed Global Variable Definitions *************************/

// User callback to see if the higher-level framework can send a packet to
// the lower-level framework to be transmitted.
extern platform_common_dev_info_t platform_common_dev_info;
extern platform_high_dev_info_t platform_high_dev_info;

/**************** Locally Scoped Global Variable Definitions *************************/

// List to hold all of the empty, free entries
static dl_list _free_queue;

// The queue_array variable is an array of lists that will be filled with queue
// entries from the free_queue list
//
// NOTE:  This implementation sparsely packs the queue_array array to allow fast
//     indexing at the cost of some wasted memory.

static _queue_t* _queue_array;
static u16 _num_queues;

// Total number of queue entries
static volatile u32 _total_queue_entries;

/*****************************************************************************/
/**
 * @brief  Initialize the queue framework
 *
 * The number of queue elements we can initialize is limited by the smaller of two values:
 *     (1) The number of dl_entry structs we can squeeze into QUEUE_DL_ENTRY_MEM_SIZE
 *     (2) The number of QUEUE_BUFFER_SIZE MPDU buffers we can squeeze into QUEUE_BUFFER_SIZE
 *
 * @param  u8 dram_present        - Flag to indicate if DRAM is present
 *                                  (1 = present; other = not present)
 *
 *****************************************************************************/
void queue_init() {
    u32 i;
    dl_entry* dl_entry_base;

    // Set the total number of supported queue entries
    _total_queue_entries = WLAN_MIN((QUEUE_DL_ENTRY_MEM_SIZE / sizeof(dl_entry)), // Max dl_entry
                                    (QUEUE_BUFFER_SIZE / QUEUE_ELEMENT_SIZE));    // Max buffers

    // Initialize the queue_array
    //     NOTE:  queue_array is initially NULL because it will be dynamically allocated and
    //         initialized.
    //
    _queue_array = NULL;
    _num_queues = 0;

    // Initialize the free queue
    dl_list_init(&_free_queue);

    // Zero all queue elements
    bzero((void*)QUEUE_BUFFER_BASE, QUEUE_BUFFER_SIZE);

    // Allocate the memory space into queue entries
    //
    // All queue entries are initially part of the free queue.  Each queue entry consists of:
    //     (1) Buffer to hold data
    //     (2) dl_entry to describe the buffer
    //
    // NOTE:  The code below exploits the fact that the starting state of all queue entries is
    //     sequential.  Therefore, it can use matrix addressing.  Matrix addressing is not safe
    //     once the queue is used and the insert/remove helper functions should be used instead.
    //
    dl_entry_base = (dl_entry*)(QUEUE_DL_ENTRY_MEM_BASE);

    for (i = 0; i < _total_queue_entries; i++) {
        // Segment the buffer in QUEUE_BUFFER_SIZE pieces
        dl_entry_base[i].data = (void*)(QUEUE_BUFFER_BASE + (i * QUEUE_ELEMENT_SIZE));

        // Copy the pointer to the dl_entry into the DRAM payload. This will allow any context
        // (like Ethernet Rx) to find the dl_entry that points to a given DRAM payload
        ((pkt_queue_buffer_t*)(dl_entry_base[i].data))->pyld_queue_hdr.dle = &(dl_entry_base[i]);

        // Insert new dl_entry into the free queue
        dl_entry_insertEnd(&_free_queue, &(dl_entry_base[i]));
    }

    // Print status
    xil_printf("Allocated %d queue buffers in DRAM using %d kB\n", _total_queue_entries,
                                                           ((_total_queue_entries * QUEUE_ELEMENT_SIZE) / 1024));
}

/*****************************************************************************/
/**
 * @brief Open a queue
 *
 * This function will open a queue and return a QID that can be used by the
 * calling context to enqueue and dequeue items.
 *
 * @param function_ptr_t queue_state_change_callback - function that the framework
 *        should call when the newly-opened queue transitions from empty to
 *        non-empty or vice versa
 *
 * @param u32 callback_arg - argument provided to callback
 *
 * @param u32 max_len - maximum length of queue. 0 means unlimited.
 *
 * @return int -1 if error, QID otherwise
 *
 *****************************************************************************/
int queue_open(function_ptr_t queue_state_change_callback, u32 callback_arg, u32 max_len){
    u32 i;

    // First, we will search through the existing array to see if any queues
    //  have been closed and can be re-used

    for(i = 0; i < _num_queues; i++){
        if( (_queue_array[i].flags & _QUEUE_FLAGS_OPEN) == 0){
            _queue_array[i].flags |= _QUEUE_FLAGS_OPEN;
            _queue_array[i].queue_state_change_callback = queue_state_change_callback;
            _queue_array[i].callback_arg = callback_arg;
            _queue_array[i].max_len = max_len;
            return i;
        }
    }

    // Otherwise, we need to reallocate the array to make a new queue

    _queue_array = wlan_mac_high_realloc(_queue_array, ((_num_queues + 1) * sizeof(_queue_t)));

    if(_queue_array == NULL){
        xil_printf("Queue error: Unable to reallocate queue array. Check heap usage\n");
        return WLAN_FAILURE;
    }

    i = _num_queues;

    // Increment the number of queues
    _num_queues++;


    // Initialize the dl_list in the newly allocated space
    dl_list_init(&(_queue_array[i].list));

    // Set queue metadata
    _queue_array[i].flags |= _QUEUE_FLAGS_OPEN;
    _queue_array[i].queue_state_change_callback = queue_state_change_callback;
    _queue_array[i].callback_arg = callback_arg;
    _queue_array[i].max_len = max_len;

    // Return the new QID (which is 0 indexed, so it is length-1)
    return (i);
}

/*****************************************************************************/
/**
 * @brief Set maximum queue length
 *
 * This function will set a new maximum queue length in the provided queue.
 *
 * @param u32 max_len - maximum length of queue
 *
 * @return int WLAN_SUCCESS or WLAN_FAILURE
 *
 *****************************************************************************/
int queue_set_max_len(int queue_id, u32 max_len){
    if ((queue_id + 1) > _num_queues) {
        xil_printf("ERROR (queue_set_max_len): queue_id %d out of range\n", queue_id);
        return WLAN_FAILURE;
    }

    _queue_array[queue_id].max_len = max_len;

    return WLAN_SUCCESS;
}

/*****************************************************************************/
/**
 * @brief Close a queue
 *
 * This function will close the provided QID so it can be re-opened in the future.
 * This function will return an error if the queue has occupancy. It will not
 * implicitly purge the contents of the queue.
 *
 * @param int queue_id - queue selection index (returned from queue_open)
 *
 * @return int WLAN_SUCCESS or WLAN_FAILURE
 *
 *****************************************************************************/
int queue_close(int queue_id){
    if ((queue_id + 1) > _num_queues) {
        xil_printf("ERROR (queue_close): queue_id %d out of range\n", queue_id);
        return WLAN_FAILURE;
    }

    if (_queue_array[queue_id].list.length != 0){
        xil_printf("Queue error: Unable to close queue_id %d because\n", queue_id);
        xil_printf(" list has %d elements that would be leaked\n", _queue_array[queue_id].list.length);
        return WLAN_FAILURE;
    }

    // Lower the QUEUE_FLAGS_OPEN flag so this QID can be re-used in the future
    _queue_array[queue_id].flags = (_queue_array[queue_id].flags & ~_QUEUE_FLAGS_OPEN);

    return WLAN_SUCCESS;
}

/*****************************************************************************/
/**
 * @brief Check if enqueue is allowed
 *
 * @param int queue_id - queue selection index (returned from queue_open)
 *
 * @return u8 1 if allowed, 0 otherwise
 *
 *****************************************************************************/
u8 queue_enqueue_allowed(int queue_id){
    if ((queue_id + 1) > _num_queues) {
        return 0;
    }

    if ((_queue_array[queue_id].flags & _QUEUE_FLAGS_OPEN) == 0){
        return 0;
    }

    if ((queue_length(queue_id) >= _queue_array[queue_id].max_len) && (_queue_array[queue_id].max_len != 0)){
        return 0;
    }

    return 1;
}

/*****************************************************************************/
/**
 * @brief  Total number of queue entries
 *
 * This is the sum of all free and occupied queue entries
 *
 * @return u32
 *
 *****************************************************************************/
u32 queue_total_size(){
    return _total_queue_entries;
}



/*****************************************************************************/
/**
 * @brief  Number of free queue entries
 *
 * @return u32
 *
 *****************************************************************************/
u32 queue_num_free(){
    return _free_queue.length;
}



/*****************************************************************************/
/**
 * @brief  Number of queue entries in a given queue
 *
 * @param  u16 queue_id - ID of the queue
 *
 * @return u32
 *
 *****************************************************************************/
u32 queue_length(u16 queue_id){
    if((queue_id+1) > _num_queues){
        return 0;
    } else {
        return _queue_array[queue_id].list.length;
    }
}

/*****************************************************************************/
/**
 * @brief  Removes all queue entries in the selected queue
 *
 * This function removes all entries from the selected queue and returns them to
 * the free pool.
 *
 * Note: if any actions need to be taken by the users of the queue prior to purging,
 * it is their responsibility to do so prior to calling this function. See
 * wlan_mac_purge_wireless_tx() as an example.
 *
 * @param  u16 queue_id - ID of the queue to purge
 *
 *****************************************************************************/
void queue_purge(u16 queue_id){
    u32 num_queued;
    u32 i;
    dl_entry* queue_entry;
    volatile interrupt_state_t prev_interrupt_state;

    num_queued = queue_length(queue_id);

    if (num_queued > 0) {
#if WLAN_SW_CONFIG_ENABLE_WLAN_EXP
        wlan_exp_printf(WLAN_EXP_PRINT_INFO, print_type_queue, "Purging %d packets from queue %d\n", num_queued, queue_id);
#endif

        // NOTE:  There is an interesting condition that can occur if an LTG is running (ie creating new TX queue
        //     entries) and purge_queue is called.  In this case, you have one process removing elements from the
        //     queue while another process is adding elements to the queue.  Therefore, purge_queue will only
        //     remove a fixed number of elements from the queue (ie all queued elements at the time the function
        //     is called).  If purge_queue used a while loop with no checking on the number of elements removed,
        //     then it could conceivably run forever.
        //
        for (i = 0; i < num_queued; i++) {
            // The queue purge is not interrupt safe
            //     NOTE:  Since there could be many elements in the queue, we need to toggle the interrupts
            //         inside the for loop so that we do not block CPU High for an extended period of time.
            //         This could result in CPU Low dropping a reception.
            //
            prev_interrupt_state = wlan_platform_intc_stop();

            queue_entry = dequeue_head(queue_id);

            if (queue_entry){
                queue_checkin(queue_entry);
            }

            // Re-enable interrupts
            wlan_platform_intc_set_state(prev_interrupt_state);
        }
    }
}

/*****************************************************************************/
/**
 * @brief  Retrieve a packet queue buffer pointer
 *
 * Returns a pointer to a packet queue buffer at a particular index. This function
 * does not remove it from the queue.
 *
 * @param  u16 queue_id - ID of the queue from which the packet will be retrieved.
 * @param  u32 idx       - Index into the queue. 0 represents the HEAD entry
 *
 * @return pkt_queue_buffer_t* - NULL if error, pointer to packet queue buffer otherwise
 *
 *****************************************************************************/
pkt_queue_buffer_t* queue_retrieve_buffer_from_index(u16 queue_id, u32 idx){
    pkt_queue_buffer_t* ret = NULL;
    dl_entry* queue_entry;
    volatile interrupt_state_t prev_interrupt_state;
    u32 num_queued, i;

    if ((queue_id + 1) > _num_queues) {
        xil_printf("ERROR (queue_retrieve_buffer_from_index 1): queue_id %d out of range\n", queue_id);
        return ret;
    }

    if (idx > (_queue_array[queue_id].list.length - 1)) {
        xil_printf("ERROR (queue_retrieve_buffer_from_index 2): idx %d is out of range\n", idx);
        return ret;
    }

    prev_interrupt_state = wlan_platform_intc_stop();
    num_queued = queue_length(queue_id);
    queue_entry = _queue_array[queue_id].list.first;
    if (num_queued > 0) {
        for (i = 0; i < num_queued; i++) {
            if(i == idx){
                break;
            }
            queue_entry = dl_entry_next(queue_entry);
        }
        if(queue_entry) ret = (pkt_queue_buffer_t*)(queue_entry->data);
    }
    // Re-enable interrupts
    wlan_platform_intc_set_state(prev_interrupt_state);

    return ret;
}

/*****************************************************************************/
/**
 * @brief  Adds a queue entry to a specified queue at the tail
 *
 * Adds the queue entry pointed to by queue_entry to the queue with ID queue_id.
 *
 * @param  u16 queue_id          - ID of the queue to which queue_entry is added.
 * @param  dl_entry* queue_entry  - Queue entry containing packet
 *
 * @return int - WLAN_SUCCESS or WLAN_FAILURE
 *
 *****************************************************************************/
int enqueue_tail(u16 queue_id, dl_entry* queue_entry){

#if ADDRESS_SAFETY_CHECKS
    if( ((u32)queue_entry < QUEUE_DL_ENTRY_MEM_BASE) || ((u32)queue_entry > CALC_MEM_HIGH_ADDR(QUEUE_DL_ENTRY_MEM_BASE,QUEUE_DL_ENTRY_MEM_SIZE)) ){
        xil_printf("%s error: dl_entry @ 0x%08x out of range\n",  __FUNCTION__, queue_entry);
    }
#endif

    if ((queue_id + 1) > _num_queues) {
        xil_printf("Queue error: queue_id %d out of range\n", queue_id);
        return WLAN_FAILURE;
    }

    if ((queue_length(queue_id) >= _queue_array[queue_id].max_len) && (_queue_array[queue_id].max_len != 0)){
        xil_printf("ERROR (enqueue_tail): queue ID %d at max occupancy (%d)\n", queue_id, _queue_array[queue_id].max_len);
        return WLAN_FAILURE;
    }

    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();

    // Insert the queue entry into the dl_list representing the selected queue
    dl_entry_insertEnd(&(_queue_array[queue_id].list), (dl_entry*)queue_entry);

    if(_queue_array[queue_id].list.length == 1){
        //If the queue element we just added is now the only member of this queue, we should inform
        //the top-level MAC that the queue has transitioned from empty to non-empty.
        _queue_array[queue_id].queue_state_change_callback(_queue_array[queue_id].callback_arg, 1);
    }

    wlan_platform_intc_set_state(curr_interrupt_state);

    return WLAN_SUCCESS;
}


/*****************************************************************************/
/**
 * @brief  Adds a queue entry to a specified queue at the head
 *
 * Adds the queue entry pointed to by queue_entry to the queue with ID queue_id.
 *
 * @param  u16 queue_id          - ID of the queue to which queue_entry is added.
 * @param  dl_entry* queue_entry  - Queue entry containing packet
 *
 * @return int - WLAN_SUCCESS or WLAN_FAILURE
 *
 *****************************************************************************/
int enqueue_head(u16 queue_id, dl_entry* queue_entry){

#if ADDRESS_SAFETY_CHECKS
    if( ((u32)queue_entry < QUEUE_DL_ENTRY_MEM_BASE) || ((u32)queue_entry > CALC_MEM_HIGH_ADDR(QUEUE_DL_ENTRY_MEM_BASE,QUEUE_DL_ENTRY_MEM_SIZE)) ){
        xil_printf("%s error: dl_entry @ 0x%08x out of range\n",  __FUNCTION__, queue_entry);
    }
#endif

    if ((queue_id + 1) > _num_queues) {
        xil_printf("Queue error: queue_id %d out of range\n", queue_id);
        return WLAN_FAILURE;
    }

    if ((queue_length(queue_id) >= _queue_array[queue_id].max_len) && (_queue_array[queue_id].max_len != 0)){
        xil_printf("ERROR (enqueue_head): queue ID %d at max occupancy (%d)\n", queue_id, _queue_array[queue_id].max_len);
        return WLAN_FAILURE;
    }

    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();

    // Insert the queue entry into the dl_list representing the selected queue
    dl_entry_insertBeginning(&(_queue_array[queue_id].list), (dl_entry*)queue_entry);

    if(_queue_array[queue_id].list.length == 1){
        //If the queue element we just added is now the only member of this queue, we should inform
        //the top-level MAC that the queue has transitioned from empty to non-empty.
        _queue_array[queue_id].queue_state_change_callback(_queue_array[queue_id].callback_arg, 1);
    }

    wlan_platform_intc_set_state(curr_interrupt_state);

    return WLAN_SUCCESS;
}


/*****************************************************************************/
/**
 * @brief  Removes the head entry from the specified queue
 *
 * If queue_id is not empty this function returns a queue_entry pointer
 * for the head entry in the queue. If the specified queue is empty this
 * function returns NULL.
 *
 * @param  u16 queue_id  - ID of the queue from which to dequeue an entry
 *
 * @return dl_entry*     - Pointer to queue entry if available,
 *                                  NULL if queue is empty
 *
 *****************************************************************************/
dl_entry* dequeue_head(u16 queue_id){
    dl_entry* curr_dl_entry;

    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();

    if ((queue_id + 1) > _num_queues) {
        wlan_platform_intc_set_state(curr_interrupt_state);
        return NULL;
    } else {
        if (_queue_array[queue_id].list.length == 0) {
            // Requested queue exists but is empty
            wlan_platform_intc_set_state(curr_interrupt_state);
            return NULL;
        } else {
            curr_dl_entry = (_queue_array[queue_id].list.first);
            dl_entry_remove(&_queue_array[queue_id].list, curr_dl_entry);

            if(_queue_array[queue_id].list.length == 0){
                //If the queue element we just removed empties the queue, we should inform
                //the top-level MAC that the queue has transitioned from non-empty to empty.
                _queue_array[queue_id].queue_state_change_callback(_queue_array[queue_id].callback_arg, 0);
            }
            wlan_platform_intc_set_state(curr_interrupt_state);

            return (dl_entry *) curr_dl_entry;
        }
    }
}

/*****************************************************************************/
/**
 * @brief  Checks out one queue entry from the free pool
 *
 * The queue framework maintains a pool of free queue entries. This function
 * removes one entry from the free pool and returns it for use by the MAC
 * application. If the free pool is empty NULL is returned.
 *
 * @return dl_entry *     - Pointer to new queue entry if available,
 *                                  NULL if free queue is empty
 *
 *****************************************************************************/
dl_entry* queue_checkout(){
    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();

    dl_entry* queue_entry;

    if(_free_queue.length > 0){
        queue_entry = ((dl_entry*)(_free_queue.first));
        dl_entry_remove(&_free_queue,_free_queue.first);

        // Set buffer length
        //  In the current architecture, this length is fixed.
        ((pkt_queue_buffer_t*)(queue_entry->data))->pyld_queue_hdr.buffer_length = QUEUE_ELEMENT_SIZE;

        wlan_platform_intc_set_state(curr_interrupt_state);
        return queue_entry;
    } else {
        wlan_platform_intc_set_state(curr_interrupt_state);
        return NULL;
    }
}



/*****************************************************************************/
/**
 * @brief  Checks in one queue entry to the free pool
 *
 * The queue framework maintains a pool of free queue entries. This function
 * returns one entry to the free pool.  queue_entry must be a valid pointer to a queue
 * entry which the MAC application no longer needs. The application must not
 * use the entry pointed to by tqe after calling this function.
 *
 * @param  dl_entry* queue_entry  - Pointer to queue entry to be returned to free pool
 *
 *****************************************************************************/
void queue_checkin(dl_entry* queue_entry){

#if ADDRESS_SAFETY_CHECKS
    if( ((u32)queue_entry < QUEUE_DL_ENTRY_MEM_BASE) || ((u32)queue_entry > CALC_MEM_HIGH_ADDR(QUEUE_DL_ENTRY_MEM_BASE,QUEUE_DL_ENTRY_MEM_SIZE)) ){
        xil_printf("%s error: dl_entry @ 0x%08x out of range\n",  __FUNCTION__, queue_entry);
    }
#endif

    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();
    if (queue_entry != NULL) {
        dl_entry_insertEnd(&_free_queue, (dl_entry*)queue_entry);
    }
    wlan_platform_free_queue_entry_notify();
    wlan_platform_intc_set_state(curr_interrupt_state);
}



/*****************************************************************************/
/**
 * @brief Checks out multiple queue entries from the free pool
 *
 * The queue framework maintains a pool of free queue entries. This function
 * attempts to check out num_queue_entry queue entries from the free pool. The number of
 * queue entries successfully checked out is returned. This may be less than
 * requested if the free pool had fewer than num_queue_entry entries available.
 *
 * @param   dl_list* new_list    - Pointer to dl_list to which queue entries are appended
 * @param   u16 num_queue_entry  - Number of queue entries requested
 *
 * @return  Number of queue entries successfully checked out and appended to new_list
 *
 *****************************************************************************/
int queue_checkout_list(dl_list* list, u16 num_queue_entry){
    int num_moved;
    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();
    num_moved = dl_entry_move(&_free_queue, list, num_queue_entry);
    wlan_platform_intc_set_state(curr_interrupt_state);
    return num_moved;
}



/*****************************************************************************/
/**
 * @brief Checks in multiple queue entries into the free pool
 *
 * The queue framework maintains a pool of free queue entries. This function will
 * check in all queue entries from the provided list to the end of the free pool.
 *
 * @param   dl_list * new_list    - Pointer to dl_list from which queue entries will be checked in
 *
 * @return  Number of queue entries successfully checked in
 *
 *****************************************************************************/
int queue_checkin_list(dl_list* list) {
    int num_moved;
    interrupt_state_t curr_interrupt_state = wlan_platform_intc_stop();
    num_moved = dl_entry_move(list, &_free_queue, list->length);
    wlan_platform_intc_set_state(curr_interrupt_state);
    wlan_platform_free_queue_entry_notify();
    return num_moved;
}