/** @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;
}