source: ReferenceDesigns/w3_802.11/c/wlan_mac_high_framework/wlan_mac_schedule.c @ 5743

/** @file wlan_mac_schedule.c
 *  @brief Scheduler
 *
 *  This set of functions allows upper-level MAC implementations
 *  to schedule the execution of a provided callback for some point
 *  in the future.
 *
 *  @copyright Copyright 2014-2017, 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"

#include "xil_types.h"
#include "xil_exception.h"

#include "xparameters.h"
#include "xtmrctr.h"
#include "xintc.h"

#include "wlan_mac_time_util.h"
#include "wlan_mac_high.h"
#include "wlan_mac_dl_list.h"
#include "wlan_mac_schedule.h"


/*************************** Constant Definitions *****************************/

#define WLAN_SCHED_EXEC_MONITOR                            0
#define WLAN_SCHED_EXEC_MONITOR_DEFAULT_THRESHOLD          100000         // 100 ms


/*************************** Variable Definitions ****************************/
static XTmrCtr               timer_instance;

volatile static u32          schedule_count;


static wlan_sched_state_t    wlan_sched_coarse;
static wlan_sched_state_t    wlan_sched_fine;
static dl_list               disabled_list;

#if WLAN_SCHED_EXEC_MONITOR
static u64                   last_exec_timestamp;
static u64                   schedule_exec_monitor;
static u32                   monitor_threshold;
#endif


/*************************** Function Prototypes *****************************/

void          timer_interrupt_handler(void * instancePtr);
void          schedule_handler(void * callback_ref, u8 timer_number);


/******************************** Functions **********************************/

/*****************************************************************************/
/**
 * Initializes the schedulers.
 *
 * @param    None
 *
 * @return   None
 *
 *****************************************************************************/
int wlan_mac_schedule_init(){
    int status;

    // Initialize internal variables
    schedule_count           = 1;

#if WLAN_SCHED_EXEC_MONITOR
    last_exec_timestamp      = 0;
    schedule_exec_monitor    = 0;
    monitor_threshold        = WLAN_SCHED_EXEC_MONITOR_DEFAULT_THRESHOLD;
#endif

    dl_list_init(&(wlan_sched_coarse.enabled_list));
    wlan_sched_coarse.next = NULL;
    dl_list_init(&(wlan_sched_fine.enabled_list));
    wlan_sched_fine.next = NULL;

    dl_list_init(&disabled_list);

    // Initialize the timer
    // The driver for the timer does not handle a reboot of CPU_HIGH
    // gracefully. Before initializing it, we should stop any running
    // timers. Note: we cannot use the official XTmrCtr_Stop() function
    // for this since it requires that the timer instance be initialized.
    u32 ControlStatusReg;
    u8  i;
    XTmrCtr_Config *TmrCtrConfigPtr = XTmrCtr_LookupConfig(TMRCTR_DEVICE_ID);

    if(TmrCtrConfigPtr != NULL){
        for(i=0; i<2; i++){
            //Loop over timer instances
            /*
             * Read the current register contents
             */
            ControlStatusReg = XTmrCtr_ReadReg(TmrCtrConfigPtr->BaseAddress,
                                        i, XTC_TCSR_OFFSET);

            /*
             * Disable the timer counter such that it's not running
             */
            ControlStatusReg &= ~(XTC_CSR_ENABLE_TMR_MASK);

            /*
             * Write out the updated value to the actual register.
             */
            XTmrCtr_WriteReg(TmrCtrConfigPtr->BaseAddress, i,
                      XTC_TCSR_OFFSET, ControlStatusReg);

        }



        status = XTmrCtr_Initialize(&timer_instance, TMRCTR_DEVICE_ID);
        if ( (status != XST_SUCCESS) ) {
            xil_printf("ERROR:  XTmrCtr failed to initialize\n");
            return -1;
        }

        // Set the handler for Timer
        XTmrCtr_SetHandler(&timer_instance, schedule_handler, &timer_instance);

        // Enable interrupt of timer and auto-reload so it continues repeatedly
        XTmrCtr_SetOptions(&timer_instance, TIMER_CNTR_FAST, XTC_DOWN_COUNT_OPTION | XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION);
        XTmrCtr_SetOptions(&timer_instance, TIMER_CNTR_SLOW, XTC_DOWN_COUNT_OPTION | XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION);

        return 0;
    } else {
        //Could not find XTmrCtr_Config for the timer
        return -1;
    }
}



/*****************************************************************************/
/**
 * Set up scheduler interrupt
 *
 * @param   intc             - pointer to instance of interrupt controller
 *
 * @return  status           - success or failure of attempt to connect
 *
 *****************************************************************************/
int wlan_mac_schedule_setup_interrupt(XIntc* intc){
    int status;

    status =  XIntc_Connect(intc, TMRCTR_INTERRUPT_ID, (XInterruptHandler)timer_interrupt_handler, &timer_instance);
    XIntc_Enable(intc, TMRCTR_INTERRUPT_ID);

    return status;
}



/*****************************************************************************/
/**
 * Schedules the execution of a callback for some time in the future
 *
 * @param   scheduler_sel    - SCHEDULE_COARSE or SCHEDULE_FINE
 * @param   delay            - Interval (in microseconds) until callback should be called
 * @param   num_calls        - Number of repetitions or SCHEDULE_REPEAT_FOREVER for permanent periodic
 * @param   callback         - Function pointer to callback
 *
 * @return  id               - ID of scheduled event or SCHEDULE_FAILURE if error
 *
 *****************************************************************************/
u32 wlan_mac_schedule_event_repeated(u8 scheduler_sel, u32 delay, u32 num_calls, void(*callback)()){
    u32            id;
    dl_entry     * entry_ptr;
    wlan_sched   * sched_ptr;
    u64            curr_system_time;

    // Allocate memory for data structures
    entry_ptr = wlan_mac_high_malloc(sizeof(dl_entry));
    if (entry_ptr == NULL) { return SCHEDULE_FAILURE; }

    sched_ptr = wlan_mac_high_malloc(sizeof(wlan_sched));
    if (sched_ptr == NULL) { wlan_mac_high_free(entry_ptr); return SCHEDULE_FAILURE; }

    // Attach the schedule struct to this dl_entry
    entry_ptr->data = sched_ptr;

    do{
        // Get Schedule ID from global counter
        id = (schedule_count++);

        // Check if we hit the section of reserved IDs; Wrap back to 0 and start again
        if ((id >= SCHEDULE_ID_RESERVED_MIN) && (id <= SCHEDULE_ID_RESERVED_MAX)) { id = 1; }

        // Check to make sure that this id has not been issued to any currently
        // scheduled events
    } while( (wlan_mac_schedule_find(&wlan_sched_coarse.enabled_list, id) != NULL) ||
             (wlan_mac_schedule_find(&wlan_sched_fine.enabled_list, id) != NULL) ||
             (wlan_mac_schedule_find(&disabled_list, id) != NULL));

    // Get the current system time
    curr_system_time     = get_system_time_usec();

    // Initialize the schedule struct
    sched_ptr->enabled   = 1;
    sched_ptr->id        = id;
    sched_ptr->delay_us  = delay;
    sched_ptr->num_calls = num_calls;
    sched_ptr->target_us = curr_system_time + (u64)(delay);
    sched_ptr->callback  = (function_ptr_t)callback;

    // Add schedule struct to the appropriate schedule list
    switch(scheduler_sel){
        // ------------------------------------------------
        case SCHEDULE_COARSE:
            // Start timer if the list goes from 0 -> 1 event
            if (wlan_sched_coarse.enabled_list.length == 0) {
                XTmrCtr_SetResetValue(&timer_instance, TIMER_CNTR_SLOW, (SLOW_TIMER_DUR_US * TIMER_CLKS_PER_US));
                XTmrCtr_Start(&timer_instance, TIMER_CNTR_SLOW);
            }

            dl_entry_insertBeginning(&(wlan_sched_coarse.enabled_list), entry_ptr);
        break;

        // ------------------------------------------------
        case SCHEDULE_FINE:
            // Start timer if the list goes from 0 -> 1 event
            if(wlan_sched_fine.enabled_list.length == 0){
                XTmrCtr_SetResetValue(&timer_instance, TIMER_CNTR_FAST, (FAST_TIMER_DUR_US * TIMER_CLKS_PER_US));
                XTmrCtr_Start(&timer_instance, TIMER_CNTR_FAST);
            }

            dl_entry_insertBeginning(&(wlan_sched_fine.enabled_list), entry_ptr);
        break;

        // ------------------------------------------------
        default:
            xil_printf("Unknown scheduler selection.  No event scheduled.\n");
            return SCHEDULE_FAILURE;
        break;
    }

    return id;
}


dl_entry* wlan_mac_schedule_disable_id(u8 scheduler_sel, u32 sched_id){
    dl_entry* sched_entry;
    dl_list* enabled_list;
    u8 timer_sel;

    switch(scheduler_sel){
        case SCHEDULE_COARSE:
            enabled_list = &(wlan_sched_coarse.enabled_list);
            timer_sel = TIMER_CNTR_SLOW;
        break;

        case SCHEDULE_FINE:
            enabled_list = &(wlan_sched_fine.enabled_list);
            timer_sel = TIMER_CNTR_FAST;
        break;

        default:
            return NULL;
        break;
    }

    sched_entry = wlan_mac_schedule_find(enabled_list, sched_id);

    if(sched_entry != NULL){
        if( ((wlan_sched*)(sched_entry->data))->num_calls != 0 ){
            dl_entry_remove(enabled_list, sched_entry);
            dl_entry_insertEnd(&disabled_list, sched_entry);
            ((wlan_sched*)(sched_entry->data))->enabled = 0;
        } else {
            // wlan_mac_schedule_disable_id() is not supported for a schedule whose num_calls
            // is 0. The most common occurence of this scenario is trying to disable a schedule
            // from the final execution of a callback of that schedule.
            return NULL;
        }
    }

    // Stop the timer if there are no more events
    //     NOTE:  Will be restarted when an event is added or re-enabled
    if (enabled_list->length == 0) {
        XTmrCtr_Stop(&timer_instance, timer_sel);
    }

    return sched_entry;
}

int wlan_mac_schedule_enable(u8 scheduler_sel, dl_entry* sched_entry){

    dl_list* enabled_list;
    u8 timer_sel;
    u32 reset_value;

    if(sched_entry == NULL){
        return -1;
    }
    switch(scheduler_sel){
        case SCHEDULE_COARSE:
            enabled_list = &(wlan_sched_coarse.enabled_list);
            timer_sel = TIMER_CNTR_SLOW;
            reset_value = (SLOW_TIMER_DUR_US * TIMER_CLKS_PER_US);
        break;

        case SCHEDULE_FINE:
            enabled_list = &(wlan_sched_fine.enabled_list);
            timer_sel = TIMER_CNTR_FAST;
            reset_value = (FAST_TIMER_DUR_US * TIMER_CLKS_PER_US);
        break;

        default:
            return -1;
        break;
    }

    dl_entry_remove(&disabled_list, sched_entry);
    dl_entry_insertEnd(enabled_list, sched_entry);

    ((wlan_sched*)(sched_entry->data))->enabled = 1;
    ((wlan_sched*)(sched_entry->data))->target_us = get_system_time_usec() + ((wlan_sched*)(sched_entry->data))->delay_us;

    // Start timer if the list goes from 0 -> 1 event
    if(wlan_sched_fine.enabled_list.length == 1){
        XTmrCtr_SetResetValue(&timer_instance, timer_sel, reset_value);
        XTmrCtr_Start(&timer_instance, timer_sel);
    }

    return 0;

}


/*****************************************************************************/
/**
 * @brief  Cancels the execution of a scheduled callback
 *
 * @param   scheduler_sel    - SCHEDULE_COARSE or SCHEDULE_FINE
 * @param   id               - ID of schedule that should be removed
 *
 * @return  None
 *
 * @note    This function will fail silently if the ID parameter does not match
 *          any currently running schedule event IDs.
 *
 *****************************************************************************/
void wlan_mac_remove_schedule(u8 scheduler_sel, u32 id){
    dl_entry     * curr_entry_ptr;
    wlan_sched   * curr_sched_ptr;

    dl_list* enabled_list;

    switch(scheduler_sel){
        case SCHEDULE_COARSE:
            enabled_list = &(wlan_sched_coarse.enabled_list);
        break;

        case SCHEDULE_FINE:
            enabled_list = &(wlan_sched_fine.enabled_list);
        break;

        default:
            return;
        break;
    }

    curr_entry_ptr = wlan_mac_schedule_find(enabled_list, id);

    if (curr_entry_ptr != NULL) {

        // Extract the schedule struct from dl_entry
        curr_sched_ptr = (wlan_sched*)(curr_entry_ptr->data);

        switch (scheduler_sel) {
            // ------------------------------------------------
            case SCHEDULE_COARSE:
                if(curr_sched_ptr != NULL){
                    if(wlan_sched_coarse.next == curr_entry_ptr){
                        // The next dl_entry in the schedule_handler iteration
                        // is the schedule we are about to remove. We need to advance
                        // this pointer prior to removing so that we do not break
                        // the execution of schedule_handler.
                        wlan_sched_coarse.next = dl_entry_next(curr_entry_ptr);
                    }
                    dl_entry_remove(&(wlan_sched_coarse.enabled_list), curr_entry_ptr);
                    wlan_mac_high_free(curr_entry_ptr);
                    wlan_mac_high_free(curr_sched_ptr);
                }

                // Stop the timer if there are no more events
                //     NOTE:  Will be restarted when new event is added
                if (wlan_sched_coarse.enabled_list.length == 0) {
                    XTmrCtr_Stop(&timer_instance, TIMER_CNTR_SLOW);
                }
            break;

            // ------------------------------------------------
            case SCHEDULE_FINE:
                if(curr_sched_ptr != NULL){
                    if(wlan_sched_fine.next == curr_entry_ptr){
                        // The next dl_entry in the schedule_handler iteration
                        // is the schedule we are about to remove. We need to advance
                        // this pointer prior to removing so that we do not break
                        // the execution of schedule_handler.
                        wlan_sched_fine.next = dl_entry_next(curr_entry_ptr);
                    }
                    dl_entry_remove(&(wlan_sched_fine.enabled_list), curr_entry_ptr);
                    wlan_mac_high_free(curr_entry_ptr);
                    wlan_mac_high_free(curr_sched_ptr);
                }

                // Stop the timer if there are no more events
                //     NOTE:  Will be restarted when new event is added
                if (wlan_sched_fine.enabled_list.length == 0){
                    XTmrCtr_Stop(&timer_instance, TIMER_CNTR_FAST);
                }
            break;

            // ------------------------------------------------
            default:
                xil_printf("Unknown scheduler selection.  No event removed.\n");
            break;
        }

#if WLAN_SCHED_EXEC_MONITOR
        // If both timers are stopped, then we need to reset the last_exec_timestamp
        //     NOTE:  This is so we do not get erroneous results when the timer is off
        //         for extended periods of time.
        if ((wlan_sched_coarse.length == 0) && (wlan_sched_fine.length == 0)) {
            last_exec_timestamp = 0;
        }
#endif
    }
}



/*****************************************************************************/
/**
 * Timer interrupt handler
 *
 * @param   instance_ptr     - Pointer to the timer instance
 *
 * @return  None
 *
 * @note    This function is a modified implementation of XTmrCtr_InterruptHandler
 *          in the xtmrctr_intr.c driver. It has been modified to remove an explicit
 *          reset of the timer.
 *
 *****************************************************************************/
void timer_interrupt_handler(void * instance_ptr){

    XTmrCtr      * timer_ptr;
    u8             timer_number;
    u32            base_addr;
    u32            csr_reg;

#ifdef _ISR_PERF_MON_EN_
    wlan_mac_set_dbg_hdr_out(ISR_PERF_MON_GPIO_MASK);
#endif

    // Verify inputs are valid
    if (instance_ptr == NULL) { return; }

    timer_ptr = (XTmrCtr *) instance_ptr;
    base_addr = timer_ptr->BaseAddress;

    // Loop thru each timer counter in the device and call the callback
    // function for each timer which has caused an interrupt
    for (timer_number = 0; timer_number < XTC_DEVICE_TIMER_COUNT; timer_number++) {

        // Read Control / Status register
        csr_reg = XTmrCtr_ReadReg(base_addr, timer_number, XTC_TCSR_OFFSET);

        // Check if interrupt is enabled
        if (csr_reg & XTC_CSR_ENABLE_INT_MASK) {

            // Check if timer expired and interrupt occured
            if (csr_reg & XTC_CSR_INT_OCCURED_MASK) {
                // Increment statistics for the number of interrupts
                timer_ptr->Stats.Interrupts++;

                // Call callback to handle timer processing
                timer_ptr->Handler(timer_ptr->CallBackRef, timer_number);

                // Read Control / Status register
                csr_reg = XTmrCtr_ReadReg(base_addr, timer_number, XTC_TCSR_OFFSET);

                // Acknowledge the interrupt
                //     NOTE: It's very important that this ack occurs after the handler call. This 
                //         will prevent a scenario where a fine schedule callback can "crash" a 
                //         node by taking longer than whatever the interval is of the fast timer.
                XTmrCtr_WriteReg(base_addr, timer_number, XTC_TCSR_OFFSET, (csr_reg | XTC_CSR_INT_OCCURED_MASK));
            }
        }
    }

#ifdef _ISR_PERF_MON_EN_
    wlan_mac_clear_dbg_hdr_out(ISR_PERF_MON_GPIO_MASK);
#endif
}



/*****************************************************************************/
/**
 * Internal callback used by the timer interrupt handler. This function should
 * not be called by the upper-level MAC.
 *
 * @param   callback_ref   - Reserved (Not currently used)
 * @param   timer_number   - ID of which timer has expired
 *
 * @return  None
 *
 *****************************************************************************/
void schedule_handler(void * callback_ref, u8 timer_number){
    dl_entry*           curr_entry_ptr;
    wlan_sched*         curr_sched_ptr;

    u8                  scheduler;
    volatile wlan_sched_state_t* wlan_sched_state;
    u32                 sched_id;
    function_ptr_t      sched_callback;

    u64            curr_system_time;

    volatile static         u8 debug_print = 0;

    // Get current system time
    curr_system_time = get_system_time_usec();

#if WLAN_SCHED_EXEC_MONITOR
    // ----------------------------------------------------
    // Scheduler Monitor Implementation
    //
    // Collect the "time between executions" of the schedule_handler function.
    //
    // The first time through the handler after the timer has been started,
    // the last_exec_timestamp will be zero.  Therefore, we want to wait until
    // subsequent loops to get the "time between executions" (ie the value
    // recorded by the schedule_exec_monitor variable).
    //
    if (last_exec_timestamp != 0) {
        schedule_exec_monitor = curr_system_time - last_exec_timestamp;
    }

    // If the monitor threshold is exceeded, print a warning.
    //     NOTE:  Since prints take a long time, update the system time so the
    //         print does not affect the collection of "time between executions"
    if (schedule_exec_monitor > monitor_threshold) {
        xil_printf("WARNING:  %d us between scheduler executions.\n", schedule_exec_monitor);
        curr_system_time = get_system_time_usec();
    }

    // Store the current time in the last_timer_timestamp
    last_exec_timestamp = curr_system_time;
#endif

    // Set any timer specific variables
    if (timer_number == TIMER_CNTR_FAST) {
        scheduler  = SCHEDULE_FINE;
        wlan_sched_state = &(wlan_sched_fine);
    } else {
        // All other timers default to coarse scheduler
        scheduler  = SCHEDULE_COARSE;
        wlan_sched_state = &(wlan_sched_coarse);
    }

    // Get the first entry of the schedule dl_list
    wlan_sched_state->next = wlan_sched_state->enabled_list.first;

    // Update the appropriate scheduler
    while (wlan_sched_state->next != NULL) {
        curr_entry_ptr = wlan_sched_state->next;
        wlan_sched_state->next = dl_entry_next(wlan_sched_state->next);
        curr_sched_ptr = (wlan_sched*)(curr_entry_ptr->data);

        if(debug_print){
            xil_printf("curr_sched_ptr = 0x%08x\n", curr_sched_ptr);
            xil_printf("curr_sched_ptr->callback = 0x%08x\n", curr_sched_ptr->callback);
            xil_printf("curr_sched_ptr->id = %d\n", curr_sched_ptr->id);
        }

        // If the target time has passed, then process the event
        if (curr_system_time >= (curr_sched_ptr->target_us)) {
            sched_id       = curr_sched_ptr->id;
            sched_callback = curr_sched_ptr->callback;

            // Update the number of scheduled event calls
            if ((curr_sched_ptr->num_calls != SCHEDULE_REPEAT_FOREVER) &&
                (curr_sched_ptr->num_calls != 0)) {
                (curr_sched_ptr->num_calls)--;
            }

            sched_callback(sched_id);

            // The update of the target or removal of the schedule occurs after the callback so that the callback
            // has the opportunity to "save" the schedule. It can do this by updating "num_calls" or by calling
            // wlan_mac_schedule_disable
            if(curr_sched_ptr->enabled == 1){
                if(curr_sched_ptr->num_calls == 0){
                    // Remove event (stops timer if necessary)
                    wlan_mac_remove_schedule(scheduler, sched_id);
                } else {
                    // Update scheduled event for next execution
                    curr_sched_ptr->target_us = curr_system_time + (u64)(curr_sched_ptr->delay_us);
                }
            }
        }
    }
}



/*****************************************************************************/
/**
 * Find schedule that corresponds to a given ID
 *
 * @param   dl_list*        - List to search through; typically the fine, coarse, and deactivated lists
 * @param   id              - ID of the scheduler that should be returned
 *
 * @return  dl_entry*       - Pointer to the list entry that contains the schedule
 *
 *****************************************************************************/
dl_entry* wlan_mac_schedule_find(dl_list* sched_list, u32 id){
    int            iter;

    dl_entry     * curr_dl_entry;
    dl_entry     * next_dl_entry;
    wlan_sched   * curr_wlan_sched;

    // Initialize the loop variables
    iter          = sched_list->length;
    next_dl_entry = sched_list->first;

    // Process the list
    while ((next_dl_entry != NULL) && (iter-- > 0)) {
        curr_dl_entry   = next_dl_entry;
        next_dl_entry   = dl_entry_next(next_dl_entry);
        curr_wlan_sched = (wlan_sched*)(curr_dl_entry->data);

        if(curr_wlan_sched->id == id){
            return curr_dl_entry;
        }
    }

    return NULL;
}