/space/home/slher/rel/Beken/v6_4_5/macsw/ip/lmac/src/tdls/tdls.c

Go to the documentation of this file.

/*
 * INCLUDE FILES
 ****************************************************************************************
 */
#include "mac_frame.h"
#include "mm.h"
#include "mm_timer.h"
#include "mm_bcn.h"
#include "co_endian.h"
#include "sta_mgmt.h"
#include "vif_mgmt.h"
#include "phy.h"
#include "dbg.h"
#include "rd.h"
#include "ps.h"
#include "txl_cntrl.h"
#include "txl_frame.h"
#include "rxl_cntrl.h"
#include "hal_machw.h"
#include "scan.h"
#include "chan.h"
#include "hal_dma.h"
#if (NX_P2P)
#include "p2p.h"
#endif //(NX_P2P)
#if (NX_TD)
#include "td.h"
#endif //(NX_TD)
#if (RW_BFMER_EN)
#include "bfr.h"
#endif //(RW_BFMER_EN)

#include "reg_mac_core.h"
#include "reg_mac_pl.h"

#include "tdls.h"
#if (NX_UMAC_PRESENT)
#include "me_utils.h"
#endif

// For IE access functions
#include "mac_ie.h"


#if NX_UMAC_PRESENT && NX_TDLS

static void tdls_keep_alive_frame_tx_cfm(void *env, uint32_t status)
{
    struct sta_info_tag *sta_tdls = (struct sta_info_tag *)env;

    // If TX is not successful, inform the host
    if (!(status & FRAME_SUCCESSFUL_TX_BIT))
    {
        sta_tdls->tdls.ka_retries--;
        if (sta_tdls->tdls.ka_retries == 0)
        {
            struct vif_info_tag *vif = &vif_info_tab[sta_tdls->inst_nbr];
            // Inform host that the TDLS peer is unreachable
            mm_send_pktloss_ind(vif, sta_tdls->staid,
                                TDLS_KA_RETRIES * RC_MAX_NUM_RETRY * RATE_CONTROL_STEPS);
        }
        else
        {
            // Retry to send the keep-alive QoS Null Frame
            txl_frame_send_qosnull_frame(sta_tdls->staid, 0x5, tdls_keep_alive_frame_tx_cfm, sta_tdls);
        }
    }

    sta_tdls->tdls.traffic_available = false;
}

static void tdls_chsw_null_frame_tx_cfm(void *env, uint32_t status)
{
    struct sta_info_tag *sta = env;
    struct vif_info_tag *vif = &vif_info_tab[sta->inst_nbr];

    // If TX is successful stop Channel Switch Timeout timer
    if (status & FRAME_SUCCESSFUL_TX_BIT)
    {
        PROF_TDLS_CHSW_TIMEOUT_TIMER_CLR();
        mm_timer_clear(&sta->tdls.chsw_timeout_timer);
    }

    // Send the postponed frames
    sta_mgmt_send_postponed_frame(vif, sta, 0);
}

static void tdls_chsw_req_tx_cfm(void *env, uint32_t status)
{
    // If TX is not successful set state TDLS_BASE_CHANNEL
    if (!(status & FRAME_SUCCESSFUL_TX_BIT))
    {
        ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
    }
}

static void tdls_chsw_rsp_tx_cfm(void *env, uint32_t status)
{
    struct vif_info_tag *vif = (struct vif_info_tag *)env;

     //Switch to the new channel just after ack received
    if (status & FRAME_SUCCESSFUL_TX_BIT)
    {
        struct mm_remain_on_channel_req param;
        struct sta_info_tag *sta = &sta_info_tab[vif->u.sta.tdls_chsw_sta_idx];

        PROF_TDLS_CHSW_TIME_TIMER_SET();
        mm_timer_set(&sta->tdls.chsw_time_timer,
                ke_time() + sta->tdls.chsw_time);

        param.op_code = MM_ROC_OP_START;
        param.vif_index = vif->index;
        param.chan = sta->tdls.chsw;
        param.duration_ms = tdls_get_dt_us(vif->tbtt_timer.time, hal_machw_time()) / 1000;

        PROF_TDLS_SWITCH_TO_OFFCH_SET();
        chan_roc_req(&param, TASK_TDLS);
        ke_state_set(TASK_TDLS, TDLS_OFF_CHANNEL);
        PROF_TDLS_SWITCH_TO_OFFCH_CLR();
    }
    else
    {
        // Reset STA index
        vif->u.sta.tdls_chsw_sta_idx = INVALID_STA_IDX;
        // Reset TDLS state
        ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
    }
}

static void tdls_chsw_unsol_rsp_tx_cfm(void *env, uint32_t status)
{
    struct vif_info_tag *vif = (struct vif_info_tag *)env;
    struct mm_remain_on_channel_req param;

    param.vif_index = vif->index;
    param.op_code = MM_ROC_OP_CANCEL;

    PROF_TDLS_SWITCH_TO_BASECH_SET();
    chan_roc_req(&param, TASK_TDLS);
    PROF_TDLS_SWITCH_TO_BASECH_CLR();

    // Reset state
    ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
}

static uint16_t tdls_check_tdls_channel_switch_request(uint8_t *frame,
                                                       uint32_t offset,
                                                       struct sta_info_tag *sta_tdls,
                                                       uint32_t next_tbtt)
{
    uint16_t status = TDLS_CHANSW_REQUEST_ACCEPTED;
    uint32_t dt = tdls_get_dt_us(next_tbtt, ke_time());
    uint16_t ch_switch_time = TDLS_CHSW_SWITCH_TIME_US;
    uint16_t chsw_timeout;
    uint32_t frame_addr = CPU2HW(frame);

    do
    {
        // Get Target Channel Number
        sta_tdls->tdls.chsw_num = co_read8p(frame_addr + offset + TDLS_CHANSW_REQ_TARGET_CH_OFFSET);
        // Get Operating Class
        sta_tdls->tdls.chsw_op_class = co_read8p(frame_addr + offset + TDLS_CHANSW_REQ_OP_CLASS);
        // Set band
        if (((sta_tdls->tdls.chsw_op_class == 112) || (sta_tdls->tdls.chsw_op_class == 2) ||
             (sta_tdls->tdls.chsw_op_class == 3) || (sta_tdls->tdls.chsw_op_class == 4) ||
             (sta_tdls->tdls.chsw_op_class == 5) || (sta_tdls->tdls.chsw_op_class == 6)) &&
             (sta_tdls->tdls.chsw_num < 14))
            sta_tdls->tdls.chsw.band = PHY_BAND_5G;
        else
            sta_tdls->tdls.chsw.band = (sta_tdls->tdls.chsw_num < 14) ? PHY_BAND_2G4 : PHY_BAND_5G;
        // Set frequency
        sta_tdls->tdls.chsw.prim20_freq = phy_channel_to_freq(sta_tdls->tdls.chsw.band, sta_tdls->tdls.chsw_num);
        sta_tdls->tdls.chsw.center1_freq = sta_tdls->tdls.chsw.prim20_freq;
        sta_tdls->tdls.chsw.type = PHY_CHNL_BW_20;

        offset += TDLS_CHANSW_REQ_IES_OFFSET;
        // Get Secondary Channel Offset (optional)
        if (co_read8p(frame_addr + offset) == MAC_ELTID_SEC_CH_OFFSET)
        {
            sta_tdls->tdls.chsw.type = PHY_CHNL_BW_40;
            if (co_read8p(frame_addr + offset + MAC_INFOELT_SEC_CH_OFFSET_SEC_CH_OFT) == MAC_INFOELT_SEC_CH_OFFSET_SEC_ABOVE)
            {
                sta_tdls->tdls.chsw.center1_freq = sta_tdls->tdls.chsw.prim20_freq + 10;
            }
            else
            {
                sta_tdls->tdls.chsw.center1_freq = sta_tdls->tdls.chsw.prim20_freq - 10;
            }
            offset += TDLS_CHANSW_REQ_IE_SEC_CH_OFT_LEN;
        }

        // Get Link Identifier IE (mandatory)
        if (co_read8p(frame_addr + offset) != MAC_ELTID_LINK_IDENTIFIER)
        {
            TRACE_TDLS(ERR, "Link Identifier element missing!");

            status = TDLS_CHANSW_REQUEST_DECLINED;
            break;
        }
        // @todo Check Link Identifier information element
        offset += TDLS_CHANSW_REQ_IE_LINK_ID_LEN;

        // Get Channel Switch Timing IE (mandatory)
        if (co_read8p(frame_addr + offset) != MAC_ELTID_CHANNEL_SWITCH_TIMING)
        {
            TRACE_TDLS(ERR, "Channel Switch Timing element missing!");
            status = TDLS_CHANSW_REQUEST_DECLINED;
            break;
        }

        sta_tdls->tdls.chsw_time = co_read16p(frame_addr + offset + MAC_INFOELT_CH_SWITCH_TIMING_SWTIME_OFT);
        sta_tdls->tdls.chsw_timeout = co_read16p(frame_addr + offset + MAC_INFOELT_CH_SWITCH_TIMING_SWTOUT_OFT);
        offset += TDLS_CHANSW_REQ_IE_CH_SWITCH_TIMING_LEN;
        // Update switch time and switch timeout
        if (sta_tdls->tdls.chsw_time < ch_switch_time)
        {
            sta_tdls->tdls.chsw_time = ch_switch_time;
        }
        chsw_timeout = (uint16_t)(dt - 2*(uint32_t)sta_tdls->tdls.chsw_time -
                2*TDLS_CHSW_TX_FRAME_TIME_US);
        if (chsw_timeout > TDLS_MAX_CHSW_SWITCH_TIME_US)
        {
            chsw_timeout = TDLS_MAX_CHSW_SWITCH_TIME_US;
        }
        if (sta_tdls->tdls.chsw_timeout < chsw_timeout)
        {
            sta_tdls->tdls.chsw_timeout = chsw_timeout;
        }

        // Check if we have time to complete the channel switch
        if (dt < (2*(uint32_t)sta_tdls->tdls.chsw_time + 2*TDLS_CHSW_TX_FRAME_TIME_US +
                sta_tdls->tdls.chsw_timeout))
        {
            status = TDLS_CHANSW_REQUEST_DECLINED;
            break;
        }

        // Get Wide Bandwidth Channel Switch IE (optional)
        if (co_read8p(frame_addr + offset) == MAC_ELTID_WIDE_BANDWIDTH_CHAN_SWITCH)
        {
            sta_tdls->tdls.chsw.band = PHY_BAND_5G;
            sta_tdls->tdls.chsw.type = 1 + co_read8p(frame_addr + offset + MAC_INFOELT_WIDE_BW_CHAN_SWITCH_NEW_CW_OFT);
            sta_tdls->tdls.chsw.prim20_freq = phy_channel_to_freq(PHY_BAND_5G, sta_tdls->tdls.chsw_num);
            sta_tdls->tdls.chsw.center1_freq = co_read8p(frame_addr + offset + MAC_INFOELT_WIDE_BW_CHAN_SWITCH_NEW_CENTER1_OFT);
            sta_tdls->tdls.chsw.center2_freq = co_read8p(frame_addr + offset + MAC_INFOELT_WIDE_BW_CHAN_SWITCH_NEW_CENTER2_OFT);
        }

    } while (0);

    return status;
}

static uint32_t tdls_check_tdls_channel_switch_response(uint8_t *frame,
                                                        uint32_t offset,
                                                        struct sta_info_tag *sta_tdls,
                                                        struct mac_addr *initiator_mac_addr,
                                                        struct mac_addr *responder_mac_addr,
                                                        uint32_t next_tbtt)
{
    uint32_t res = CO_FAIL;
    uint8_t i;
    struct mac_addr initiator;
    struct mac_addr responder;
    uint32_t dt = tdls_get_dt_us(next_tbtt, ke_time());
    uint32_t frame_addr = CPU2HW(frame);

    do
    {
        // Get status
        if (co_read8p(frame_addr + offset + TDLS_CHANSW_RSP_STATUS_OFFSET) != TDLS_CHANSW_REQUEST_ACCEPTED)
        {
            res = CO_FAIL;
            break;
        }
        offset += TDLS_CHANSW_RSP_IES_OFFSET;

        // Get Link Identifier IE (mandatory)
        if (co_read8p(frame_addr + offset) != MAC_ELTID_LINK_IDENTIFIER)
        {
            TRACE_TDLS(ERR, "Link Identifier IE missing.");
            res = CO_NOT_FOUND;
            break;
        }
        // Check Link Identifier IE
        for (i = 0; i < MAC_ADDR_LEN/2; i++)
        {
            initiator.array[i] = co_read16p(frame_addr + offset + MAC_INFOELT_LINK_ID_INIT_STA_OFT + 2*i);
            responder.array[i] = co_read16p(frame_addr + offset + MAC_INFOELT_LINK_ID_RESP_STA_OFT + 2*i);
        }
        if ((!MAC_ADDR_CMP(&initiator.array[0], &initiator_mac_addr->array[0])) ||
            (!MAC_ADDR_CMP(&responder.array[0], &responder_mac_addr->array[0])))
        {
            TRACE_TDLS(ERR, "wrong Link Identifier IE.");
            res = CO_BAD_PARAM;
            break;
        }
        offset += TDLS_CHANSW_REQ_IE_LINK_ID_LEN;

        // Get Channel Switch Timing IE (mandatory)
        if (co_read8p(frame_addr + offset) != MAC_ELTID_CHANNEL_SWITCH_TIMING)
        {
            TRACE_TDLS(ERR, "Channel Switch Timing IE missing.");
            res = CO_NOT_FOUND;
            break;
        }
        // Check Channel Switch Timing IE only if not an unsolicited response
        if (ke_state_get(TASK_TDLS) == TDLS_CHSW_REQ_TX)
        {
            // Update time and timeout
            sta_tdls->tdls.chsw_time = co_read16p(frame_addr + offset + MAC_INFOELT_CH_SWITCH_TIMING_SWTIME_OFT);
            sta_tdls->tdls.chsw_timeout = co_read16p(frame_addr + offset + MAC_INFOELT_CH_SWITCH_TIMING_SWTOUT_OFT);
            // Check if we have time to complete the channel switch
            if (dt < (2*(uint32_t)sta_tdls->tdls.chsw_time + TDLS_CHSW_TX_FRAME_TIME_US +
                    sta_tdls->tdls.chsw_timeout))
            {
                TRACE_TDLS(ERR, "Not enough time to complete TDLS Channel Switch procedure.");
                res = CO_BAD_PARAM;
                break;
            }
        }
        offset += TDLS_CHANSW_REQ_IE_CH_SWITCH_TIMING_LEN;

        res = CO_OK;

    } while (0);

    return res;
}

static uint32_t tdls_check_tdls_peer_traffic_response(uint8_t *frame,
                                                      uint32_t offset,
                                                      struct sta_info_tag *sta_tdls,
                                                      struct mac_addr *initiator_mac_addr,
                                                      struct mac_addr *responder_mac_addr)
{
    uint32_t res = CO_FAIL;
    uint8_t i;
    struct mac_addr initiator;
    struct mac_addr responder;
    uint32_t frame_addr = CPU2HW(frame);

    do
    {
        // Get Dialog Token
        if (co_read8p(frame_addr + offset + MAC_ACTION_TOKEN_OFT) != sta_tdls->tdls.dialog_token)
        {
            dbg(D_CRT "%s: wrong Dialog Token.\n", __func__);
            res = CO_FAIL;
            break;
        }
        offset += TDLS_PEER_TRAFFIC_RSP_IES_OFFSET;

        // Get Link Identifier IE (mandatory)
        if (co_read8p(frame_addr + offset) != MAC_ELTID_LINK_IDENTIFIER)
        {
            dbg(D_CRT "%s: Link Identifier IE missing.\n", __func__);
            res = CO_NOT_FOUND;
            break;
        }
        // Check Link Identifier IE
        for (i = 0; i < MAC_ADDR_LEN/2; i++)
        {
            initiator.array[i] = co_read16p(frame_addr + offset + MAC_INFOELT_LINK_ID_INIT_STA_OFT + 2*i);
            responder.array[i] = co_read16p(frame_addr + offset + MAC_INFOELT_LINK_ID_RESP_STA_OFT + 2*i);
        }
        if ((!MAC_ADDR_CMP(&initiator.array[0], &initiator_mac_addr->array[0])) ||
            (!MAC_ADDR_CMP(&responder.array[0], &responder_mac_addr->array[0])))
        {
            dbg(D_CRT "%s: wrong Link Identifier IE.\n", __func__);
            res = CO_BAD_PARAM;
            break;
        }

        res = CO_OK;

    } while (0);

    return res;
}

static void tdls_chsw_end_evt(void *env)
{
    struct vif_info_tag *vif = (struct vif_info_tag *)env;

    PROF_TDLS_CHSW_END_TIMER_CLR();

    // Send Unsolicited TDLS Channel Switch Response to indicate we go back to the
    // base channel
    txl_frame_send_tdls_channel_switch_rsp_frame(vif, 0, &tdls_chsw_unsol_rsp_tx_cfm);
}

static void tdls_chsw_time_evt(void *env)
{
    struct vif_info_tag *vif = (struct vif_info_tag *)env;
    struct sta_info_tag *sta;

    PROF_TDLS_CHSW_TIME_TIMER_CLR();

    if ((chan_is_on_channel(vif)) &&
        (ke_state_get(TASK_TDLS) == TDLS_OFF_CHANNEL) &&
        (vif->u.sta.tdls_chsw_sta_idx != INVALID_STA_IDX))
    {
        // Start Channel Switch Timeout timer
        PROF_TDLS_CHSW_TIMEOUT_TIMER_SET();
        sta = &sta_info_tab[vif->u.sta.tdls_chsw_sta_idx];
        mm_timer_set(&sta->tdls.chsw_timeout_timer,
                     ke_time() + sta->tdls.chsw_timeout);

        // Send Channel Switch Indication
        struct tdls_chan_switch_ind *ind = KE_MSG_ALLOC(TDLS_CHAN_SWITCH_IND,
                TASK_API, TASK_TDLS,
                tdls_chan_switch_ind);
        ind->vif_index = vif->index;
        ind->chan_ctxt_index = vif->chan_ctxt->idx;
        ind->status = CO_OK;

        ke_msg_send(ind);

        // Update RC with new configuration
        me_sta_bw_nss_max_upd(sta->staid, sta->tdls.chsw.type, 0xFF);

        // Set timer for end of period on off channel
        sta->tdls.chsw_end_timer.cb = tdls_chsw_end_evt;
        sta->tdls.chsw_end_timer.env = vif;
        PROF_TDLS_CHSW_END_TIMER_SET();
        mm_timer_set(&sta->tdls.chsw_end_timer,
                     vif->tbtt_timer.time - 15000);

        // Notify presence on the off channel (only if we are TDLS channel switch initiator)
        //if (sta->tdls.chsw_initiator == false)
        {
            PROF_TDLS_CHSW_NULL_FRAME_TX_SET();
            txl_frame_send_qosnull_frame(sta->staid, 0x5, tdls_chsw_null_frame_tx_cfm, sta);
            PROF_TDLS_CHSW_NULL_FRAME_TX_CLR();
        }

        ke_state_set(TASK_TDLS, TDLS_OFF_CHANNEL);
    }
    else
    {
        struct mm_remain_on_channel_req param;

        param.vif_index = vif->index;
        param.op_code = MM_ROC_OP_CANCEL;

        PROF_TDLS_SWITCH_TO_BASECH_SET();
        chan_roc_req(&param, TASK_TDLS);
        PROF_TDLS_SWITCH_TO_BASECH_CLR();

        ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);

        PROF_TDLS_CHSW_END_TIMER_CLR();
        if (vif->u.sta.tdls_chsw_sta_idx != INVALID_STA_IDX) {
            sta = &sta_info_tab[vif->u.sta.tdls_chsw_sta_idx];
            mm_timer_clear(&sta->tdls.chsw_end_timer);
        }
    }
}

static void tdls_chsw_timeout_evt(void *env)
{
    struct vif_info_tag *vif = (struct vif_info_tag *)env;
    struct sta_info_tag *sta = &sta_info_tab[vif->u.sta.tdls_chsw_sta_idx];;
    struct mm_remain_on_channel_req param;

    PROF_TDLS_CHSW_TIMEOUT_TIMER_CLR();

    PROF_TDLS_CHSW_TIME_TIMER_CLR();
    mm_timer_clear(&sta->tdls.chsw_time_timer);
    PROF_TDLS_CHSW_END_TIMER_CLR();
    mm_timer_clear(&sta->tdls.chsw_end_timer);
    // If no successfull frame exchange within Switch Timeout, switch back to base channel
    param.vif_index = vif->index;
    param.op_code = MM_ROC_OP_CANCEL;

    PROF_TDLS_SWITCH_TO_BASECH_SET();
    chan_roc_req(&param, TASK_TDLS);
    PROF_TDLS_SWITCH_TO_BASECH_CLR();

    ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
}

static void tdls_keep_alive_evt(void *env)
{
    struct sta_info_tag *sta_tdls = (struct sta_info_tag *)env;
    struct vif_info_tag *vif = &vif_info_tab[sta_tdls->inst_nbr];

    sta_tdls->tdls.ka_retries = TDLS_KA_RETRIES;

    if (sta_tdls->ps_state == PS_MODE_ON)
    {
        if (!sta_tdls->tdls.pti_timer_running)
        {
            // Send Peer Traffic Indication frame
            struct tdls_peer_traffic_ind_req param;
            param.vif_index = vif->index;
            MAC_ADDR_CPY(&param.peer_mac_addr, &sta_tdls->mac_addr);
            param.sta_idx = sta_tdls->staid;
            param.dialog_token = 0;
            param.last_sn = 0;
            param.last_tid = 0;
            if (txl_frame_send_tdls_peer_traffic_ind_frame(&param, &tdls_peer_traffic_ind_tx_cfm) == CO_OK)
            {
                // set dialog token
                sta_tdls->tdls.dialog_token = param.dialog_token;
                // Keep in mind that a keep-alive frame should be transmitted
                sta_tdls->tdls.ka_tx_req = true;
                // set TDLS STA status
                ke_state_set(TASK_TDLS, TDLS_TRAFFIC_IND_TX);
            }
        }
    }
    else
    {
        // Send the keep-alive QoS Null Frame
        txl_frame_send_qosnull_frame(sta_tdls->staid, 0x5, tdls_keep_alive_frame_tx_cfm, sta_tdls);
    }

    // Set next keep-alive QoS Null Frame
    mm_timer_set(&sta_tdls->tdls.keep_alive_timer, ke_time() + TDLS_KEEP_ALIVE_US);
}

static void tdls_pti_timeout_evt(void *env)
{
    struct sta_info_tag *sta_tdls = (struct sta_info_tag *)env;
    struct vif_info_tag *vif = &vif_info_tab[sta_tdls->inst_nbr];
    // Inform host that the TDLS peer is unreachable
    mm_send_pktloss_ind(vif, sta_tdls->staid, RC_MAX_NUM_RETRY * RATE_CONTROL_STEPS);
}

static bool tdls_check_frame_action(uint8_t *frame, struct vif_info_tag *vif, uint8_t sta_idx)
{
    bool upload = true;
    uint16_t framectrl = co_read16(frame);
    uint32_t offset;
    uint8_t tdls_action;
    uint32_t frame_addr = CPU2HW(frame);
    struct sta_info_tag *sta = &sta_info_tab[sta_idx];

    do
    {
        if ((framectrl & MAC_FCTRL_TYPE_MASK) == MAC_FCTRL_DATA_T)
        {
            if ((ke_state_get(TASK_TDLS) == TDLS_OFF_CHANNEL) &&
                (!hal_machw_time_past(sta->tdls.chsw_timeout_timer.time)))
            {
                PROF_TDLS_CHSW_TIMEOUT_TIMER_CLR();
                mm_timer_clear(&sta->tdls.chsw_timeout_timer);
            }

            if (!(framectrl & MAC_QOS_ST_BIT))
            {
                break;
            }
        }

        offset = MAC_SHORT_QOS_MAC_HDR_LEN + ((framectrl & MAC_FCTRL_ORDER) ? MAC_HTC_LEN : 0);
        offset += (framectrl & MAC_FCTRL_PROTECTEDFRAME)?IV_LEN + EIV_LEN:0;

        if (!((co_read32p(frame_addr + offset) == FRAME_BODY_LLC_H) &&
              (co_read32p(frame_addr + offset + MAC_ENCAPSULATED_LLC_L_OFT) == FRAME_BODY_LLC_L) &&
              (co_read8p(frame_addr + offset + MAC_ENCAPSULATED_PAYLOAD_TYPE_OFT) == PAYLOAD_TYPE_TDLS)))
        {
            break;
        }

        offset += MAC_ENCAPSULATED_PAYLOAD_OFT;
        if (co_read8p(frame_addr + offset) != MAC_TDLS_ACTION_CATEGORY)
        {
            break;
        }

        tdls_action = co_read8p(frame_addr + offset + MAC_ACTION_ACTION_OFT);

        if (tdls_action == MAC_TDLS_ACTION_PEER_TRAFFIC_RSP)
        {
            // Peer Traffic Response received  //

            uint16_t res;
            upload = false;
            struct mac_addr *initiator_mac_addr;
            struct mac_addr *responder_mac_addr;

            // Check if TDLS Peer Traffic Indication frame has been sent to AP
            if (ke_state_get(TASK_TDLS) != TDLS_TRAFFIC_IND_TX)
            {
                break;
            }

            // Set TDLS link initiator and responder
            if (sta->tdls.initiator)
            {
                initiator_mac_addr = &sta->mac_addr;
                responder_mac_addr = &vif->mac_addr;
            }
            else
            {
                initiator_mac_addr = &vif->mac_addr;
                responder_mac_addr = &sta->mac_addr;
            }
            res = tdls_check_tdls_peer_traffic_response(frame, offset, sta,
                                                        initiator_mac_addr,
                                                        responder_mac_addr);

            if (res == CO_OK)
            {
                uint16_t qos = 0x5 << MAC_QOSCTRL_UP_OFT;
                // Stop PTI Response timeout timer
                mm_timer_clear(&sta->tdls.pti_timeout_timer);
                sta->tdls.pti_timer_running = false;
                // Inform the host that the TDLS peer is awake
                tdls_send_peer_ps_ind(vif, false, sta_idx);
                // Check if a keep-alive frame is waiting to be transmitted
                if (sta->tdls.ka_tx_req)
                {
                    if (!sta->tdls.traffic_available)
                    {
                        qos |= MAC_QOSCTRL_EOSP;
                    }
                    // Send QoS Null Frame
                    txl_frame_send_qosnull_frame(sta->staid, qos,
                                                 tdls_keep_alive_frame_tx_cfm, sta);
                    sta->tdls.ka_tx_req = false;
                }
                // Set state
                ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
            }
        }
        else
        if (tdls_action == MAC_TDLS_ACTION_CHANSW_REQ)
        {
            // Channel Switch Request received //
            PROF_TDLS_CHSW_REQ_RX_SET();

            uint16_t status;
            upload = false;

            // If Channel Switch Request is received while waiting for a Channel Switch Response,
            // the STA should reply only if it's the TDLS responder STA
            if ((ke_state_get(TASK_TDLS) == TDLS_CHSW_REQ_TX) && (!sta->tdls.initiator))
            {
                TRACE_TDLS(CHSWITCH, "Do not reply (STA is initiator of the TDLS link and"
                      " ChSwReq received while waiting for ChSwResp).");
                break;
            }

            // Init timers
            sta->tdls.chsw_time_timer.cb = tdls_chsw_time_evt;
            sta->tdls.chsw_time_timer.env = vif;
            sta->tdls.chsw_timeout_timer.cb = tdls_chsw_timeout_evt;
            sta->tdls.chsw_timeout_timer.env = vif;
            // Set TX power
            sta->tdls.chsw.tx_power = vif->tx_power;
            // Set the TDLS channel switch initiator
            sta->tdls.chsw_initiator = true;
            // Set TDLS STA index for channel switch response
            vif->u.sta.tdls_chsw_sta_idx = sta_idx;

            status = tdls_check_tdls_channel_switch_request(frame, offset,
                                                            sta,
                                                            vif->tbtt_timer.time);
            PROF_TDLS_CHSW_REQ_RX_CLR();

            if (status == TDLS_CHANSW_REQUEST_ACCEPTED)
            {
                // Check if there is traffic with the AP or if TDLS peer is in power save
                if ((td_sta_has_traffic(vif->u.sta.ap_id)) || (sta->ps_state))
                {
                    status = TDLS_CHANSW_REQUEST_DECLINED;
                }
                //check if Channel Switch is allowed
                else if (!sta->tdls.chsw_allowed)
                {
                    status = TDLS_CHANSW_REQUEST_DECLINED;
                }
                else
                {
                    // Set TDLS state
                    ke_state_set(TASK_TDLS, TDLS_CHSW_REQ_RX);
                }
            }

            // Send TDLS Channel Switch Response
            if (txl_frame_send_tdls_channel_switch_rsp_frame(vif, status, &tdls_chsw_rsp_tx_cfm) != CO_OK)
            {
                // Reset sta idx in case of Channel Switch refused
                vif->u.sta.tdls_chsw_sta_idx = INVALID_STA_IDX;
            }
        }
        else if (tdls_action == MAC_TDLS_ACTION_CHANSW_RSP)
        {
            // Channel Switch Response received //
            PROF_TDLS_CHSW_RESP_RX_SET();

            uint32_t res;
            upload = false;
            struct mac_addr *initiator_mac_addr;
            struct mac_addr *responder_mac_addr;

            // Check if request is from the correct TDLS STA
            if (sta_idx != vif->u.sta.tdls_chsw_sta_idx)
            {
                break;
            }

            // Init timers
            sta->tdls.chsw_time_timer.cb = tdls_chsw_time_evt;
            sta->tdls.chsw_time_timer.env = vif;
            sta->tdls.chsw_timeout_timer.cb = tdls_chsw_timeout_evt;
            sta->tdls.chsw_timeout_timer.env = vif;

            // Set TDLS link initiator and responder
            if (sta->tdls.initiator)
            {
                initiator_mac_addr = &sta->mac_addr;
                responder_mac_addr = &vif->mac_addr;
            }
            else
            {
                initiator_mac_addr = &vif->mac_addr;
                responder_mac_addr = &sta->mac_addr;
            }
            res = tdls_check_tdls_channel_switch_response(frame, offset,
                                                          sta,
                                                          initiator_mac_addr, responder_mac_addr,
                                                          vif->tbtt_timer.time);
            PROF_TDLS_CHSW_RESP_RX_CLR();

            // Switch to the new channel just after Channel Switch Response frame has been received,
            // if Channel Switch Request frame is correct
            if (res != CO_OK)
            {
                if (ke_state_get(TASK_TDLS) == TDLS_CHSW_REQ_TX)
                {
                    ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
                }
            }
            else
            {
                struct mm_remain_on_channel_req param;

                param.vif_index = vif->index;
                param.chan = sta->tdls.chsw;
                param.duration_ms = tdls_get_dt_us(vif->tbtt_timer.time, ke_time()) / 1000;

                // do not switch to the new channel if already on the requested channel
                if ((vif->chan_ctxt->channel.band != sta->tdls.chsw.band) ||
                    (vif->chan_ctxt->channel.prim20_freq != sta->tdls.chsw.prim20_freq) ||
                    (vif->chan_ctxt->channel.type != sta->tdls.chsw.type))
                {
                    // switch to the off channel if we sent a Channel Switch Request
                    if (ke_state_get(TASK_TDLS) == TDLS_CHSW_REQ_TX)
                    {
                        PROF_TDLS_CHSW_TIME_TIMER_SET();
                        mm_timer_set(&sta->tdls.chsw_time_timer,
                                ke_time() + sta->tdls.chsw_time);

                        param.op_code = MM_ROC_OP_START;

                        PROF_TDLS_SWITCH_TO_OFFCH_SET();
                        chan_roc_req(&param, TASK_TDLS);
                        PROF_TDLS_SWITCH_TO_OFFCH_CLR();

                        ke_state_set(TASK_TDLS, TDLS_OFF_CHANNEL);
                    }
                }
                // switch to base channel is unsolicited Channel Switch Response frame received
                else if (ke_state_get(TASK_TDLS) == TDLS_OFF_CHANNEL)
                {
                    PROF_TDLS_CHSW_TIME_TIMER_CLR();
                    mm_timer_clear(&sta->tdls.chsw_time_timer);
                    PROF_TDLS_CHSW_END_TIMER_CLR();
                    mm_timer_clear(&sta->tdls.chsw_end_timer);
                    param.op_code = MM_ROC_OP_CANCEL;
                    PROF_TDLS_SWITCH_TO_BASECH_SET();
                    chan_roc_req(&param, TASK_TDLS);
                    PROF_TDLS_SWITCH_TO_BASECH_CLR();

                    ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
                }
            }
        }
    } while(0);

    return upload;
}

static void tdls_check_peer_ps(uint8_t *frame, struct vif_info_tag *vif, uint8_t sta_idx)
{
    struct mac_hdr *machdr = (struct mac_hdr *)frame;
    uint16_t framectrl = machdr->fctl;
    struct sta_info_tag *sta = &sta_info_tab[sta_idx];
    bool ps_on = true;

    do
    {
        // Check frame control NULL function
        if (!(framectrl & MAC_QOS_ST_BIT))
        {
            break;
        }
        // Check From DS and To DS flags
        if ((framectrl & MAC_FCTRL_TODS_FROMDS) != 0)
        {
            break;
        }

        // Check Power Management flag
        if (!(framectrl & MAC_FCTRL_PWRMGT))
        {
            ps_on = false;
        }

        if (ps_on != sta->ps_state)
        {
            // Inform the host that the TDLS peer PS state is changed
            tdls_send_peer_ps_ind(vif, ps_on, sta_idx);
        }
        // If TDLS peer enables PS when we are on the off channel, send a Unsolicited
        // Channel Switch Response to go back to base channel
        if (ps_on && (ke_state_get(TASK_TDLS) == TDLS_OFF_CHANNEL))
        {
            txl_frame_send_tdls_channel_switch_rsp_frame(vif, 0, &tdls_chsw_unsol_rsp_tx_cfm);
        }
    } while (0);
}

void tdls_chsw_req_evt(void *env)
{
    struct vif_info_tag *vif = (struct vif_info_tag *)env;
    struct sta_info_tag *sta;
    struct tdls_chan_switch_req req;
    uint32_t next_evt;

    PROF_TDLS_CHSW_REQ_TX_TIMER_CLR();

    do
    {
        if (vif->u.sta.tdls_chsw_sta_idx == INVALID_STA_IDX)
        {
            break;
        }

        sta = &sta_info_tab[vif->u.sta.tdls_chsw_sta_idx];

        // Set next ChSw Request event to the next TBTT
        next_evt = vif->tbtt_timer.time + TDLS_CHSW_REQ_DELAY_US;
        // Send the TDLS Channel Switch Request frame if there isn't traffic with the AP
        // and if the TDLS peer is not in power save
        if ((!td_sta_has_traffic(vif->u.sta.ap_id)) && (sta->ps_state == PS_MODE_OFF))
        {
            // Check if we are on the base channel
            if (ke_state_get(TASK_TDLS) == TDLS_BASE_CHANNEL)
            {
                req.vif_index = vif->index;
                req.sta_idx = sta->staid;
                MAC_ADDR_CPY(&req.peer_mac_addr, &sta->mac_addr);
                req.initiator = sta->tdls.initiator;
                req.chan = sta->tdls.chsw;
                req.op_class = sta->tdls.chsw_op_class;

                ke_state_set(TASK_TDLS, TDLS_CHSW_REQ_TX);

                // send TDLS Channel Switch Request frame
                txl_frame_send_tdls_channel_switch_req_frame(&req, &tdls_chsw_req_tx_cfm);
            }
            // Check if we are waiting to complete TDLS channel switch procedure
            // (i.e. TDLS ChSw Response not yet received): reset the state and wait the next
            // TBTT to try again the Channel Switch
            else if (ke_state_get(TASK_TDLS) == TDLS_CHSW_REQ_TX)
            {
                ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
            }
            PROF_TDLS_DELAY_CHSW_CLR();
        }

        // Set next channel switch request
        PROF_TDLS_CHSW_REQ_TX_TIMER_SET();
        mm_timer_set(&sta->tdls.chsw_req_timer, next_evt);

    } while(0);
}

bool tdls_check_frame(uint8_t *frame, struct vif_info_tag *vif, uint8_t sta_idx)
{
    bool upload;

    // Check if the TDLS peer is going to sleep
    tdls_check_peer_ps(frame, vif, sta_idx);

    // Check if TDLS Channel Switch Request, TDLS Channel Switch Response,
    // TDLS Peer Traffic Response and update TDLS info
    upload = tdls_check_frame_action(frame, vif, sta_idx);

    return upload;
}

void tdls_send_chan_switch_base_ind(struct chan_ctxt_tag *roc_chan_ctxt)
{
    struct vif_info_tag *vif = &vif_info_tab[roc_chan_ctxt->vif_index];
    struct sta_info_tag *sta = &sta_info_tab[vif->u.sta.tdls_chsw_sta_idx];
    // Inform the host that the remain on channel has expired
    struct tdls_chan_switch_base_ind *ind = KE_MSG_ALLOC(TDLS_CHAN_SWITCH_BASE_IND,
                                                         TASK_API, roc_chan_ctxt->taskid,
                                                         tdls_chan_switch_base_ind);
    ind->chan_ctxt_index = roc_chan_ctxt->idx;
    ind->vif_index       = roc_chan_ctxt->vif_index;

    PROF_TDLS_SWITCH_TO_BASECH_SET();
    ke_msg_send(ind);
    PROF_TDLS_SWITCH_TO_BASECH_CLR();

    // Update RC with new configuration
    me_sta_bw_nss_max_upd(sta->staid, vif->chan_ctxt->channel.type, 0xFF);

    // Reset tdls_chsw_sta_idx and timers if TDLS Channel Switch is not active
    if (vif->u.sta.tdls_chsw_sta_idx != INVALID_STA_IDX)
    {
        if (sta->tdls.chsw_active == false)
        {
            vif->u.sta.tdls_chsw_sta_idx = INVALID_STA_IDX;
            PROF_TDLS_CHSW_TIME_TIMER_CLR();
            mm_timer_clear(&sta->tdls.chsw_time_timer);
            PROF_TDLS_CHSW_END_TIMER_CLR();
            mm_timer_clear(&sta->tdls.chsw_end_timer);
        }
    }

    // Send the postponed frames
    sta_mgmt_send_postponed_frame(vif, sta, 0);

    ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
}

void tdls_send_peer_ps_ind(struct vif_info_tag *vif, bool ps_on, uint8_t sta_idx)
{
    struct sta_info_tag *sta = &sta_info_tab[sta_idx];

    // Inform the host that the TDLS peer is going to sleep or is awake
    struct tdls_peer_ps_ind *ind = KE_MSG_ALLOC(TDLS_PEER_PS_IND,
                                                TASK_API, TASK_TDLS,
                                                tdls_peer_ps_ind);

    // Change the local value of the PS state
    sta->ps_state = ps_on;

    ind->vif_index = vif->index;
    ind->sta_idx = sta->staid;
    MAC_ADDR_CPY(ind->peer_mac_addr.array, &sta->mac_addr);
    ind->ps_on = ps_on;

    ke_msg_send(ind);
}

void tdls_add_sta(struct sta_info_tag *sta, bool initiator, bool chsw_allowed)
{
    sta->tdls.initiator = initiator;
    sta->tdls.chsw_allowed = chsw_allowed;
    // Init the keep-alive timer for the TDLS Peer
    sta->tdls.keep_alive_timer.cb = tdls_keep_alive_evt;
    sta->tdls.keep_alive_timer.env = sta;
    mm_timer_set(&sta->tdls.keep_alive_timer, ke_time() + TDLS_KEEP_ALIVE_US);
    // Init PTI Response timeout timer
    sta->tdls.pti_timeout_timer.cb = tdls_pti_timeout_evt;
    sta->tdls.pti_timeout_timer.env = sta;
}

void tdls_del_sta(struct sta_info_tag *sta)
{
    struct vif_info_tag *vif = &vif_info_tab[sta->inst_nbr];

    // Clear keep-alive timer
    mm_timer_clear(&sta->tdls.keep_alive_timer);
    // Clear PTI Response timeout timer
    mm_timer_clear(&sta->tdls.pti_timeout_timer);
    // Clear channel switch request timer
    mm_timer_clear(&sta->tdls.chsw_req_timer);
    // Clear channel switch time timer
    mm_timer_clear(&sta->tdls.chsw_time_timer);
    // Clear channel switch timeout timer
    mm_timer_clear(&sta->tdls.chsw_timeout_timer);
    // Clear channel switch end timer
    mm_timer_clear(&sta->tdls.chsw_end_timer);
    sta->tdls.chsw_active = false;
    if (vif->u.sta.tdls_chsw_sta_idx == sta->staid)
    {
        vif->u.sta.tdls_chsw_sta_idx = INVALID_STA_IDX;
    }
    sta->tdls.chsw_initiator = false;
}

void tdls_peer_traffic_ind_tx_cfm(void *env, uint32_t status)
{
    struct sta_info_tag *sta_tdls = (struct sta_info_tag *)env;

    if (status & FRAME_SUCCESSFUL_TX_BIT)
    {
        // If TX is successful set PTI timeout
        mm_timer_set(&sta_tdls->tdls.pti_timeout_timer, ke_time() + TDLS_PTI_RSP_TOUT_US);
        sta_tdls->tdls.pti_timer_running = true;
    }
    else
    {
        // If TX is not successful set state TDLS_BASE_CHANNEL
        ke_state_set(TASK_TDLS, TDLS_BASE_CHANNEL);
    }
}

#endif // NX_UMAC_PRESENT && NX_TDLS

---