linux-nv-oot/drivers/net/wireless/realtek/rtl8852ce/phl/phl_twt.c

/******************************************************************************
 *
 * Copyright(c) 2019 Realtek Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
 * more details.
 *
 *****************************************************************************/
#define _PHL_TWT_C_
#include "phl_headers.h"

#ifdef CONFIG_PHL_TWT
#include "phl_twt.h"

static bool _twt_valid(struct phl_info_t *phl)
{
	bool ret = false;

	if (false == twt_sup(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_sup == false\n",
			__FUNCTION__);
		goto _exit;
	}
	if (false == twt_init(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_init == false\n",
			__FUNCTION__);
		goto _exit;
	}
	ret = true;
_exit:
	return ret;
}

void _twt_transfer_config_state(enum phl_twt_action action,
				enum twt_config_state *state)
{
	if (PHL_TWT_ACTION_FREE == action)
		*state = twt_config_state_free;
	else if (PHL_TWT_ACTION_ALLOC == action)
		*state = twt_config_state_idle;
	else if (PHL_TWT_ACTION_ENABLE == action)
		*state = twt_config_state_enable;
	else if (PHL_TWT_ACTION_DISABLE == action)
		*state = twt_config_state_idle;
	else if (PHL_TWT_ACTION_UP_ERROR == action)
		*state = twt_config_state_error;
}

/*
 * Calculate map of macid
 * @map_offset: number of offset for wait_macid_map
 * @macid_map: map of macid
 * Ex: macid_map = 0x80(bit7), offset = 2, macid 71(7 + 2*32) wait announce
 */
void _twt_calc_macid_map_info(u16 macid, u8 *map_offset, u32 *macid_map)
{
	*map_offset = (u8)(macid / 32);
	*macid_map = BIT(macid % 32);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_calc_macid_map_info(): macid:%d, map_offset:%d, macid_map:0x%x\n",
		macid, *map_offset, *macid_map);
}

u32 _twt_calc_intvl(u8 exp, u16 mantissa)
{
	u32 intvl = 0;

	intvl = mantissa * (1 << exp);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_calc_intvl(): exp:%u, mantissa:%u, intvl=%u\n",
		exp, mantissa, intvl);
	return intvl;
}

u32 _twt_calc_wakeup_dur(u8 dur, enum rtw_phl_wake_dur_unit dur_unit)
{
	u32 dur_t = 0;

	if (RTW_PHL_WAKE_256US == dur_unit)
		dur_t = dur * 256;
	else if (RTW_PHL_WAKE_1TU == dur_unit)
		dur_t = dur * 1024;
	return dur_t;
}

static void
_dump_twt_info(struct rtw_phl_twt_info_f *twt_info, const char *caller)
{
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "=========_dump_twt_info- %s====\n",
		caller);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "id(%d), rsp_req(%d), next_req(%d), next_size(%d), all_twt(%d), next_twt(0x%08X 0x%08X)\n",
		twt_info->twt_flow_id, twt_info->rsp_req,
		twt_info->next_twt_req, twt_info->next_twt_size,
		twt_info->all_twt, (u32)(twt_info->next_twt >> 32),
		(u32)twt_info->next_twt);
}

static void
_dump_btwt_para_set(struct rtw_phl_bcast_twt_para_set *para, const char *caller)
{
	struct rtw_phl_btwt_req_type *req_type = NULL;
	struct rtw_phl_btwt_i *btwt_i = NULL;

	req_type = &para->req_type;
	btwt_i = &para->btwt_i;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "=========_dump_btwt_para_set- %s====\n",
		caller);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "twt_request(%d), twt_setup_cmd(%d), trigger(%d), lst_bc_para_set(%d), flow_type(%d), btwt_rcmd(%d), twt_wake_int_exp(%d), rsvd(%d)\n",
		req_type->twt_request, req_type->twt_setup_cmd,
		req_type->trigger, req_type->lst_bc_para_set,
		req_type->flow_type, req_type->btwt_rcmd,
		req_type->twt_wake_int_exp, req_type->rsvd);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "target_wake_t(0x%08x-0x%08x), nom_min_twt_wake_dur(%d), twt_wake_int_mantissa(%d), rsvd(%d), btwt_id(%d), btwt_prstnc(%d)\n",
		para->target_wake_t_h, para->target_wake_t_l,
		para->nom_min_twt_wake_dur, para->twt_wake_int_mantissa,
		btwt_i->rsvd, btwt_i->btwt_id, btwt_i->btwt_prstnc);
}

static void
_dump_ctrl_i(struct rtw_phl_twt_control *ctrl, const char *caller)
{

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: twt_ctrl: ndp_paging_indic(%d), responder_pm_mode(%d), nego_type(%d), twt_info_frame_disable(%d), wake_dur_unit(%d)\n",
		caller, ctrl->ndp_paging_indic, ctrl->responder_pm_mode,
		ctrl->nego_type, ctrl->twt_info_frame_disable,
		ctrl->wake_dur_unit);
}

enum rtw_phl_status _twt_fill_btwt_para_set(
	struct rtw_phl_bcast_twt_para_set *para, u8 *buf, u8 *len)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_btwt_req_type *req_type = &para->req_type;
	struct rtw_phl_btwt_i *btwt_i = &para->btwt_i;

	_dump_btwt_para_set(para, __FUNCTION__);
	/*Request Type*/
	SET_BTWT_REQ_TYPE_TWT_REQUEST(buf, req_type->twt_request);
	SET_BTWT_REQ_TYPE_TWT_SETUP_COMMAND(buf, req_type->twt_setup_cmd);
	SET_BTWT_REQ_TYPE_TRIGGER(buf, req_type->trigger);
	SET_BTWT_REQ_TYPE_LST_BC_PARA_SET(buf, req_type->lst_bc_para_set);
	SET_BTWT_REQ_TYPE_FLOW_TYPE(buf, req_type->flow_type);
	SET_BTWT_REQ_TYPE_RCMD(buf, req_type->btwt_rcmd);
	SET_BTWT_REQ_TYPE_TWT_WAKE_INTERVAL_EXPONENT(buf,
					req_type->twt_wake_int_exp);
	*len = REQUEST_TYPE_LENGTH;

	SET_BTWT_TARGET_WAKE_TIME(buf + (*len), (u16)para->target_wake_t_l);
	*len += BTWT_TARGET_WAKE_TIME_LENGTH;

	SET_BTWT_NOMINAL_MINIMUM_TWT_WAKE_DURATION(buf + (*len),
						para->nom_min_twt_wake_dur);
	*len += NOMINAL_MINIMUM_TWT_WAKE_DURATION_LENGTH;

	SET_BTWT_TWT_WAKE_INTERVAL_MANTISSA(buf + (*len),
					para->twt_wake_int_mantissa);
	*len += TWT_WAKE_INTERVAL_MANTISSA_LENGTH;
	/* BTWT Info */
	SET_BTWT_ID(buf + (*len), btwt_i->btwt_id);
	SET_BTWT_PRSTNC(buf + (*len), btwt_i->btwt_prstnc);
	*len += BTWT_INFO_LENGTH;
	pstatus = RTW_PHL_STATUS_SUCCESS;
	return pstatus;
}

enum rtw_phl_status _twt_fill_individual_twt_para_set(
				struct rtw_phl_indiv_twt_para_set *para,
				bool ndp_paging, u8 *buf, u8 *length)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_req_type_indiv *req_type = &para->req_type;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_fill_individual_twt_para_set(): twt_request(%d), twt_setup_cmd(%d), trigger(%d), implicit(%d), flow_type(%d), twt_flow_id(%d), twt_wake_int_exp(%d), twt_protection(%d)\n",
		req_type->twt_request, req_type->twt_setup_cmd,
		req_type->trigger, req_type->implicit,
		req_type->flow_type, req_type->twt_flow_id,
		req_type->twt_wake_int_exp, req_type->twt_protection);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_fill_individual_twt_para_set(): target_wake_t_h(0x%08x), target_wake_t_l(0x%08x), nom_min_twt_wake_dur(%d), twt_wake_int_mantissa(%d), twt_channel(%d)\n",
		para->target_wake_t_h, para->target_wake_t_l,
		para->nom_min_twt_wake_dur, para->twt_wake_int_mantissa,
		para->twt_channel);
	*length = 0;
	/*Request Type*/
	SET_TWT_REQ_TYPE_TWT_REQUEST(buf, req_type->twt_request);
	SET_TWT_REQ_TYPE_TWT_SETUP_COMMAND(buf, req_type->twt_setup_cmd);
	SET_TWT_REQ_TYPE_TRIGGER(buf, req_type->trigger);
	SET_TWT_REQ_TYPE_IMPLICIT(buf, req_type->implicit);
	SET_TWT_REQ_TYPE_FLOW_TYPE(buf, req_type->flow_type);
	SET_TWT_REQ_TYPE_TWT_FLOW_IDENTIFER(buf, req_type->twt_flow_id);
	SET_TWT_REQ_TYPE_TWT_WAKE_INTERVAL_EXPONENT(buf,
					req_type->twt_wake_int_exp);
	SET_TWT_REQ_TYPE_TWT_PROTECTION(buf, req_type->twt_protection);
	*length += REQUEST_TYPE_LENGTH;
	if (RTW_PHL_TWT_GROUPING == req_type->twt_setup_cmd) {
		/*TODO*/
	} else {
		SET_TWT_TARGET_WAKE_TIME_L(buf, para->target_wake_t_l);
		SET_TWT_TARGET_WAKE_TIME_H(buf, para->target_wake_t_h);
		*length += TARGET_WAKE_TIME_LENGTH;
	}
	SET_TWT_NOMINAL_MINIMUM_TWT_WAKE_DURATION(buf, *length,
						para->nom_min_twt_wake_dur);
	*length += NOMINAL_MINIMUM_TWT_WAKE_DURATION_LENGTH;

	SET_TWT_TWT_WAKE_INTERVAL_MANTISSA(buf, *length,
					para->twt_wake_int_mantissa);
	*length += TWT_WAKE_INTERVAL_MANTISSA_LENGTH;
	SET_TWT_TWT_CHANNEL(buf, *length, para->twt_channel);
	*length += TWT_CHANNEL_LENGTH;
	if (true == ndp_paging) {
		/*TODO*/
	}
	pstatus = RTW_PHL_STATUS_SUCCESS;
	return pstatus;
}

enum rtw_phl_status _twt_parse_btwt_para(u8 *buf, u16 len,
			struct rtw_phl_bcast_twt_para_set *para, u8 *rlen)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_btwt_req_type *req_type = &para->req_type;
	struct rtw_phl_btwt_i *btwt_i = &para->btwt_i;

	if (len < MIN_BTWT_PARA_LEN) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: pkt len(%d) < MIN_BTWT_PARA_LEN(%d)\n",
			__FUNCTION__, len, MIN_BTWT_PARA_LEN);
		return sts;
	}
	/*Request Type*/
	req_type->twt_request = GET_BTWT_REQ_TYPE_TWT_REQUEST(buf);
	req_type->twt_setup_cmd = GET_BTWT_REQ_TYPE_TWT_SETUP_COMMAND(buf);
	req_type->trigger = GET_BTWT_REQ_TYPE_TRIGGER(buf);
	req_type->lst_bc_para_set = GET_BTWT_REQ_TYPE_LST_BC_PARA_SET(buf);
	req_type->flow_type = GET_BTWT_REQ_TYPE_FLOW_TYPE(buf);
	req_type->btwt_rcmd = GET_BTWT_REQ_TYPE_RCMD(buf);
	req_type->twt_wake_int_exp = GET_BTWT_REQ_TYPE_TWT_WAKE_INTERVAL_EXPONENT(buf);
	*rlen = REQUEST_TYPE_LENGTH;

	para->target_wake_t_l = GET_BTWT_TARGET_WAKE_TIME(buf + (*rlen));
	*rlen += BTWT_TARGET_WAKE_TIME_LENGTH;

	para->nom_min_twt_wake_dur =
		GET_BTWT_NOMINAL_MINIMUM_TWT_WAKE_DURATION(buf + (*rlen));
	*rlen += NOMINAL_MINIMUM_TWT_WAKE_DURATION_LENGTH;

	para->twt_wake_int_mantissa =
			GET_BTWT_TWT_WAKE_INTERVAL_MANTISSA(buf + (*rlen));
	*rlen += TWT_WAKE_INTERVAL_MANTISSA_LENGTH;
	/* BTWT Info */
	btwt_i->btwt_id = GET_BTWT_ID(buf + (*rlen));
	btwt_i->btwt_prstnc = GET_BTWT_PRSTNC(buf + (*rlen));
	*rlen += BTWT_INFO_LENGTH;
	_dump_btwt_para_set(para, __FUNCTION__);
	sts = RTW_PHL_STATUS_SUCCESS;
	return sts;
}

enum rtw_phl_status _twt_parse_all_btwt_para(u8 *buf, u8 para_len,
	struct rtw_phl_twt_element *twt_element, u8 *rlen)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_bcast_twt_para_set *bpara = NULL;
	u8 idx = 0, plen = 0, rem_len = 0;

	*rlen = 0;
	twt_element->num_btwt_para = 0;
	rem_len = para_len;
	do {
		if (idx >= MAX_BTWT_PARA_SET) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: idx(%d) >= MAX_BTWT_PARA_SET(%d)\n",
				__FUNCTION__, idx, MAX_BTWT_PARA_SET);
			sts = RTW_PHL_STATUS_FAILURE;
			goto exit;
		}
		bpara = &twt_element->info.b_twt_para_set[idx];
		plen = 0;
		sts = _twt_parse_btwt_para(buf + (*rlen), rem_len,
					bpara, &plen);
		if (sts != RTW_PHL_STATUS_SUCCESS) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: parse btwt error\n",
				__FUNCTION__);
			goto exit;
		}
		twt_element->num_btwt_para++;
		rem_len -= plen;
		*rlen += plen;
		if (bpara->req_type.lst_bc_para_set) {
			break;
		}
		if (rem_len < MIN_BTWT_PARA_LEN) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: rem_len(%d) < MIN_BTWT_PARA_LEN(%d), can't get last para\n",
				__FUNCTION__, rem_len,
				MIN_BTWT_PARA_LEN);
			goto exit;
		}
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: parse next bpara\n",
			__FUNCTION__);
		idx++;
	} while(true);
exit:
	return sts;
}

enum rtw_phl_status _twt_parse_individual_twt_para(u8 *twt_ele, u16 length,
				struct rtw_phl_twt_element *element)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_indiv_twt_para_set *para = &element->info.i_twt_para_set;
	struct rtw_phl_req_type_indiv *req_type = &para->req_type;

	u8 *next_buf = twt_ele + ELEM_ID_LEN + ELEM_LEN_LEN + CONTROL_LENGTH;
	req_type->twt_request = GET_TWT_REQ_TYPE_TWT_REQUEST(next_buf);
	req_type->twt_setup_cmd = GET_TWT_REQ_TYPE_TWT_SETUP_COMMAND(next_buf);
	req_type->trigger = GET_TWT_REQ_TYPE_TRIGGER(next_buf);
	req_type->implicit = GET_TWT_REQ_TYPE_IMPLICIT(next_buf);
	req_type->flow_type = GET_TWT_REQ_TYPE_FLOW_TYPE(next_buf);
	req_type->twt_flow_id = GET_TWT_REQ_TYPE_TWT_FLOW_IDENTIFER(next_buf);
	req_type->twt_wake_int_exp =
			GET_TWT_REQ_TYPE_TWT_WAKE_INTERVAL_EXPONENT(next_buf);
	req_type->twt_protection = GET_TWT_REQ_TYPE_TWT_PROTECTION(next_buf);
	next_buf += REQUEST_TYPE_LENGTH;
	if (RTW_PHL_TWT_GROUPING == req_type->twt_setup_cmd) {
		//Todo
	} else {
		para->target_wake_t_l = GET_TWT_TARGET_WAKE_TIME_L(next_buf);
		para->target_wake_t_h = GET_TWT_TARGET_WAKE_TIME_H(next_buf);
		next_buf += TARGET_WAKE_TIME_LENGTH;
	}
	para->nom_min_twt_wake_dur =
			GET_TWT_NOMINAL_MINIMUM_TWT_WAKE_DURATION(next_buf);
	next_buf += NOMINAL_MIN_TWT_WAKE_DURATION_LENGTH;
	para->twt_wake_int_mantissa =
				GET_TWT_TWT_WAKE_INTERVAL_MANTISSA(next_buf);
	next_buf += TWT_WAKE_INTERVAL_MANTISSA_LENGTH;
	para->twt_channel = GET_TWT_TWT_CHANNEL(next_buf);
	next_buf += TWT_CHANNEL_LENGTH;
	if (element->twt_ctrl.ndp_paging_indic) {
		/*TODO*/
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_parse_individual_twt_para(): twt_request:%d, twt_setup_cmd:%d, trigger:%d, implicit:%d, flow_type:%d, twt_flow_id:%d, twt_wake_int_exp:%d, twt_protection:%d\n",
		req_type->twt_request, req_type->twt_setup_cmd,
		req_type->trigger, req_type->implicit,
		req_type->flow_type, req_type->twt_flow_id,
		req_type->twt_wake_int_exp, req_type->twt_protection);

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_parse_individual_twt_para(): target_wake_t_h:0x%08X, target_wake_t_l:0x%08X, nom_min_twt_wake_dur:%d, twt_wake_int_mantissa:%d, twt_channel:%d\n",
		para->target_wake_t_h, para->target_wake_t_l,
		para->nom_min_twt_wake_dur, para->twt_wake_int_mantissa,
		para->twt_channel);
	pstatus = RTW_PHL_STATUS_SUCCESS;
	return pstatus;
}

enum rtw_phl_status _twt_announce_info_enqueue(struct phl_info_t *phl_info,
				struct phl_queue *twt_annc_q,
				struct _twt_announce_info *twt_annc)
{
	void *drv = phl_to_drvpriv(phl_info);

	if (!twt_annc)
		return RTW_PHL_STATUS_FAILURE;
	_os_spinlock(drv, &twt_annc_q->lock, _bh, NULL);
	list_add_tail(&twt_annc->list, &twt_annc_q->queue);
	twt_annc_q->cnt++;
	_os_spinunlock(drv, &twt_annc_q->lock, _bh, NULL);
	return RTW_PHL_STATUS_SUCCESS;
}

struct _twt_announce_info * _twt_announce_info_dequeue(
				struct phl_info_t *phl_info,
				struct phl_queue *twt_annc_q)
{
	struct _twt_announce_info *twt_annc = NULL;
	void *drv = phl_to_drvpriv(phl_info);

	_os_spinlock(drv, &twt_annc_q->lock, _bh, NULL);
	if (list_empty(&twt_annc_q->queue)) {
		twt_annc = NULL;
	} else {
		twt_annc = list_first_entry(&twt_annc_q->queue,
					struct _twt_announce_info, list);
		list_del(&twt_annc->list);
		twt_annc_q->cnt--;
	}
	_os_spinunlock(drv, &twt_annc_q->lock, _bh, NULL);
	return twt_annc;
}

enum rtw_phl_status _twt_sta_announce(struct phl_info_t *phl_info,
				struct phl_queue *annc_queue, u16 macid)
{
	enum rtw_hal_status hstatus = RTW_HAL_STATUS_FAILURE;
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	void *drv = phl_to_drvpriv(phl_info);
	struct _twt_announce_info *info = NULL;
	_os_list *annc_list = &annc_queue->queue;
	u8 offset = 0;
	u32 macid_map = 0;

	_twt_calc_macid_map_info(macid, &offset, &macid_map);
	_os_spinlock(drv, &annc_queue->lock, _bh, NULL);
	phl_list_for_loop(info, struct _twt_announce_info, annc_list, list) {
		if (NULL == info)
			break;
		if (info->map_offset != offset)
			continue;
		if (!(info->wait_macid_map & macid_map))
			continue;
		hstatus = rtw_hal_twt_sta_announce(phl_info->hal, (u8)macid);
		if (RTW_HAL_STATUS_SUCCESS == hstatus) {
			info->wait_macid_map &= (~macid_map);
			pstatus = RTW_PHL_STATUS_SUCCESS;
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_sta_announce(): rtw_hal_twt_sta_announce success, macid:%d, map_offset:%d, wait_macid_map:0x%x\n",
				macid, info->map_offset, info->wait_macid_map);
		} else {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "_twt_sta_announce(): rtw_hal_twt_sta_announce fail, macid:%d, map_offset:%d, wait_macid_map:0x%x\n",
				macid, info->map_offset, info->wait_macid_map);
		}
		break;
	}
	_os_spinunlock(drv, &annc_queue->lock, _bh, NULL);
	return pstatus;
}

enum rtw_phl_status _twt_set_sta_announce_state(struct phl_info_t *phl_info,
				struct phl_queue *annc_q, u16 macid,
				enum phl_wait_annc_type type)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	void *drv = phl_to_drvpriv(phl_info);
	struct _twt_announce_info *info = NULL;
	_os_list *annc_list = &annc_q->queue;
	u8 offset = 0;
	u32 macid_map = 0;
	u8 bset = false;

	_twt_calc_macid_map_info(macid, &offset, &macid_map);
	_os_spinlock(drv, &annc_q->lock, _bh, NULL);
	phl_list_for_loop(info, struct _twt_announce_info, annc_list, list) {
		if (NULL == info)
			break;
		if (info->map_offset != offset)
			continue;
		if (RTW_PHL_TWT_WAIT_ANNC_ENABLE == type) {
			info->wait_macid_map |= macid_map;
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_set_sta_announce_state(): set macid:%d to wait annc state, map_offset:%d, wait_macid_map:0x%x\n",
				macid, info->map_offset, info->wait_macid_map);
		} else if (RTW_PHL_TWT_WAIT_ANNC_DISABLE == type) {
			info->wait_macid_map &= (~macid_map);
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_set_sta_announce_state(): set macid:%d to annc state, map_offset:%d, wait_macid_map:0x%x\n",
				macid, info->map_offset, info->wait_macid_map);
		} else {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "_twt_set_sta_announce_state(): Unknown type:%d\n",
				type);
			break;
		}
		pstatus = RTW_PHL_STATUS_SUCCESS;
		bset = true;
		break;
	}
	_os_spinunlock(drv, &annc_q->lock, _bh, NULL);
	if (true == bset)
		goto exit;
	if (RTW_PHL_TWT_WAIT_ANNC_ENABLE == type) {
		info = _os_mem_alloc(drv, sizeof(struct _twt_announce_info));
		if (NULL == info) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "_twt_sta_wait_announce(): Fail to alloc new annc info\n");
		} else {
			info->map_offset = offset;
			info->wait_macid_map = macid_map;
			_twt_announce_info_enqueue(phl_info, annc_q, info);
			pstatus = RTW_PHL_STATUS_SUCCESS;
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_set_sta_announce_state(): add new Q and set macid:%d to annc state, map_offset:%d, wait_macid_map:0x%x\n",
				macid, info->map_offset, info->wait_macid_map);
		}
	} else if (RTW_PHL_TWT_WAIT_ANNC_DISABLE == type) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "_twt_set_sta_announce_state(): macid:%d is not in wait annc state\n",
				macid);
	} else {
		/*nothing*/
	}
exit:
	return pstatus;
}

enum rtw_phl_status rtw_phl_twt_handle_c2h_wait_annc(struct phl_info_t *phl,
				u8 *buf)
{
	struct rtw_phl_twt_wait_anno_rpt *wait_anno = NULL;
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_twt_info *phl_twt_info = get_twt_info(phl);
	struct phl_queue *annc_q = &phl_twt_info->twt_annc_queue;
	u8 i;
	bool error = false;

	if (buf == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_handle_c2h_wait_annc(): buf == NULL!\n");
		return RTW_PHL_STATUS_FAILURE;
	}

	wait_anno = (struct rtw_phl_twt_wait_anno_rpt *)buf;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "[%s]wait_case = %d, macid = %d %d %d\n",
							__func__,
							wait_anno->wait_case,
							wait_anno->macid[0],
							wait_anno->macid[1],
							wait_anno->macid[2]);

	for (i = 0; i < RTW_PHL_TWT_WAIT_ANNO_STA_NUM; i++) {
		if (IGNORE_MACID == wait_anno->macid[i])
			continue;
		pstatus = _twt_set_sta_announce_state(phl, annc_q,
						wait_anno->macid[i],
						wait_anno->wait_case);
		if (RTW_PHL_STATUS_SUCCESS != pstatus) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_handle_c2h_wait_annc(): pstatus:%d, macid: %d, fail to set sta wait announce state\n",
				pstatus, wait_anno->macid[i]);
			error = true;
		}
	}
	if (true == error)
		pstatus = RTW_PHL_STATUS_FAILURE;
	return pstatus;
}

/*
struct rtw_twt_sta_info *
_twt_get_twt_sta(
	struct phl_info_t *phl_info,
	struct phl_queue *sta_queue,
	struct rtw_phl_stainfo_t *phl_sta,
	u8 id
)
{
	void *drv = phl_to_drvpriv(phl_info);
	struct rtw_twt_sta_info *twt_sta = NULL, *ret_twt_sta = NULL;
	_os_list *sta_list = &sta_queue->queue;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> _twt_get_twt_sta()\n");
	_os_spinlock(drv, &sta_queue->lock, _bh, NULL);
	phl_list_for_loop(twt_sta, struct rtw_twt_sta_info, sta_list, list) {
		if (twt_sta == NULL)
			break;
		if (phl_sta != twt_sta->phl_sta)
			continue;
		if (DELETE_ALL != id && id != twt_sta->id)
			continue;
		ret_twt_sta = twt_sta;
	}
	_os_spinunlock(drv, &sta_queue->lock, _bh, NULL);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== _twt_get_twt_sta()\n");
	return ret_twt_sta;
}
*/

void _twt_fill_config_info_bc(struct rtw_phl_twt_info *twt_info,
				struct rtw_phl_bcast_twt_para_set *para_set)
{
	struct rtw_phl_btwt_req_type *req_type = &para_set->req_type;
	struct rtw_phl_btwt_i *btwt_i = &para_set->btwt_i;

	twt_info->trigger = req_type->trigger;
	twt_info->flow_type = req_type->flow_type;
	twt_info->bcast_twt_id = btwt_i->btwt_id;
	twt_info->implicit_lastbcast = req_type->lst_bc_para_set;
	twt_info->twt_wake_int_exp = req_type->twt_wake_int_exp;
	twt_info->twt_wake_int_mantissa = para_set->twt_wake_int_mantissa;
	twt_info->nom_min_twt_wake_dur = para_set->nom_min_twt_wake_dur;
	twt_info->target_wake_time_h = para_set->target_wake_t_h;
	twt_info->target_wake_time_l = para_set->target_wake_t_l;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: twt_info: trigger(%d), flow_type(%d), btwt_id(%d), lastbc:(%d), twt_wake_int_exp:%d, twt_wake_int_mantissa:%d, nom_min_twt_wake_dur:%d, target_wake_time(0x%08X-0x%08X)\n",
		__FUNCTION__, twt_info->trigger, twt_info->flow_type,
		twt_info->bcast_twt_id,
		twt_info->implicit_lastbcast,
		twt_info->twt_wake_int_exp, twt_info->twt_wake_int_mantissa,
		twt_info->nom_min_twt_wake_dur, twt_info->target_wake_time_h,
		twt_info->target_wake_time_l);
}

void _twt_fill_config_info_indiv(struct rtw_phl_twt_info *twt_info,
				struct rtw_phl_indiv_twt_para_set *para_set)
{
	struct rtw_phl_req_type_indiv *req_type = &para_set->req_type;

	twt_info->trigger = req_type->trigger;
	twt_info->flow_type = req_type->flow_type;
	twt_info->implicit_lastbcast = req_type->implicit;
	twt_info->twt_protection = req_type->twt_protection;
	twt_info->twt_wake_int_exp = req_type->twt_wake_int_exp;
	twt_info->twt_wake_int_mantissa = para_set->twt_wake_int_mantissa;
	twt_info->nom_min_twt_wake_dur = para_set->nom_min_twt_wake_dur;
	twt_info->target_wake_time_h = para_set->target_wake_t_h;
	twt_info->target_wake_time_l = para_set->target_wake_t_l;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: twt_info: trigger:%d, flow_type:%d, implicit_lastbcast:%d, twt_protection:%d, twt_wake_int_exp:%d, twt_wake_int_mantissa:%d, nom_min_twt_wake_dur:%d, target_wake_time_h:0x%08X, target_wake_time_l:0x%08X\n",
		__FUNCTION__, twt_info->trigger, twt_info->flow_type,
		twt_info->implicit_lastbcast, twt_info->twt_protection,
		twt_info->twt_wake_int_exp, twt_info->twt_wake_int_mantissa,
		twt_info->nom_min_twt_wake_dur, twt_info->target_wake_time_h,
		twt_info->target_wake_time_l);
}

void _twt_fill_config_info(struct rtw_phl_twt_info *twt_info,
				struct rtw_phl_twt_setup_info *setup_info)
{
	struct rtw_phl_twt_element *twt_ele = &setup_info->twt_element;
	struct rtw_phl_twt_control *twt_ctrl = &twt_ele->twt_ctrl;

	twt_info->responder_pm_mode = twt_ctrl->responder_pm_mode;
	twt_info->nego_type = twt_ctrl->nego_type;
	twt_info->wake_dur_unit = twt_ctrl->wake_dur_unit;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: twt_info: responder_pm_mode:%d, nego_type:%d, wake_dur_unit:%d\n",
		__FUNCTION__, twt_info->responder_pm_mode, twt_info->nego_type,
		twt_info->wake_dur_unit);
	if (RTW_PHL_INDIV_TWT == twt_info->nego_type) {
		_twt_fill_config_info_indiv(twt_info,
					&twt_ele->info.i_twt_para_set);
	} else if (RTW_PHL_BCAST_TWT == twt_info->nego_type ||
		   RTW_PHL_MANAGE_BCAST_TWT == twt_info->nego_type){
		_twt_fill_config_info_bc(twt_info,
					&twt_ele->info.b_twt_para_set[0]);
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Unimplemented twt_ctrl->nego_type(%d)\n",
			__FUNCTION__, twt_info->nego_type);
	}
}

void _twt_reset_config_info(struct phl_info_t *phl,
				struct phl_twt_config *config)
{
	config->role = NULL;
	config->rlink = NULL;
	_os_mem_set(phl_to_drvpriv(phl), &config->twt_info, 0,
			sizeof(struct rtw_phl_twt_info));
}

u8 _twt_compare_twt_para(struct rtw_phl_twt_info *twt_info,
				struct rtw_phl_twt_setup_info *twt_setup)
{
	u8 ret = false;
	u64 twt1 = 0, twt2 = 0, diff_t = 0;
	u32 intvl = 0;
	struct rtw_phl_twt_info cmp_info = {0};

	_twt_fill_config_info(&cmp_info, twt_setup);
	do {
		if (cmp_info.responder_pm_mode != twt_info->responder_pm_mode)
			break;
		if (cmp_info.nego_type != twt_info->nego_type)
			break;
		if (cmp_info.trigger != twt_info->trigger)
			break;
		if (cmp_info.flow_type != twt_info->flow_type)
			break;
		if (cmp_info.implicit_lastbcast != twt_info->implicit_lastbcast)
			break;
		if (cmp_info.twt_protection != twt_info->twt_protection)
			break;
		if ((_twt_calc_wakeup_dur(cmp_info.nom_min_twt_wake_dur
			, cmp_info.wake_dur_unit)) !=
			(_twt_calc_wakeup_dur(twt_info->nom_min_twt_wake_dur,
			twt_info->wake_dur_unit)))
			break;
		if ((_twt_calc_intvl(cmp_info.twt_wake_int_exp,
			cmp_info.twt_wake_int_mantissa)) !=
			(_twt_calc_intvl(twt_info->twt_wake_int_exp,
			twt_info->twt_wake_int_mantissa)))
			break;
		/*compare target wake time*/
		intvl = _twt_calc_intvl(twt_info->twt_wake_int_exp,
					twt_info->twt_wake_int_mantissa);
		twt1 = cmp_info.target_wake_time_h;
		twt1 = twt1 << 32;
		twt1 |= cmp_info.target_wake_time_l;
		twt2 = twt_info->target_wake_time_h;
		twt2 = twt2 << 32;
		twt2 |= twt_info->target_wake_time_l;
		if (twt1 > twt2) {
			/*cmp_info target_wake_time > twt_info target_wake_time*/
			diff_t = _os_minus64(twt1, twt2);
		} else {
			diff_t = _os_minus64(twt2, twt1);
		}
		if (_os_modular64(diff_t, intvl) != 0)
			break;
		ret = true;
	} while(false);
	return ret;
}

u8 _twt_is_same_config(struct phl_twt_config *config,
				struct _twt_compare *compare_info)
{
	bool found = false;

	do {
		if (config->role != compare_info->role)
			break;
		if (!(twt_config_state_idle == config->state ||
			twt_config_state_enable == config->state))
			break;
		if (_twt_compare_twt_para(&config->twt_info,
				&compare_info->twt_setup))
			found = true;
	} while(false);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== _twt_is_same_config(): found(%d)\n",
		found);
	return found;
}

void _twt_dump_twt_cfg_info(struct phl_twt_cfg_info *twt_cfg_i)
{
	struct phl_twt_config *config = NULL;
	u8 i = 0;

	config = (struct phl_twt_config *)twt_cfg_i->twt_cfg_ring;

	for (i = 0; i < twt_cfg_i->twt_cfg_num; i++) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "_twt_dump_twt_cfg_info(): loop i(%d), cfg id(%d), state(%d), role(%p), rlink(%p), StaQ(%d)\n",
			i, config[i].idx, config[i].state, config[i].role,
			config[i].rlink, config[i].twt_sta_queue.cnt);
	}
}

enum rtw_phl_status _twt_operate_twt_config(struct phl_info_t *phl_info,
		struct phl_twt_cfg_info *twt_cfg_i, enum phl_operate_config_type type,
		u8 *para, struct phl_twt_config **ret_config)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_twt_config *config =
			(struct phl_twt_config *)twt_cfg_i->twt_cfg_ring;
	u8 i = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "==> _twt_operate_twt_config(): type(%d)\n",
		type);
	if (type == PHL_GET_CONFIG_BY_ID) {
		if (*para >= twt_cfg_i->twt_cfg_num) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_operate_twt_config(): get cfg by id(%d) fail, out of range(%d)\n",
				*para, twt_cfg_i->twt_cfg_num);
			goto exit;
		}
		if (twt_config_state_free == config[*para].state){
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_operate_twt_config(): get cfg by id(%d) fail, cfg state is in twt_config_state_free\n",
				*para);
			goto exit;
		}
		*ret_config = &config[*para];
		pstatus = RTW_PHL_STATUS_SUCCESS;
		goto exit;
	} else if (type == PHL_FREE_CONFIG) {
		if (*para >= twt_cfg_i->twt_cfg_num) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_operate_twt_config(): free cfg by id(%d) fail, out of range(%d)\n",
				*para, twt_cfg_i->twt_cfg_num);
			goto exit;
		}
		if (twt_config_state_free == config[*para].state){
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_operate_twt_config(): free cfg by id(%d) fail, cfg state is in twt_config_state_free\n",
				*para);
			goto exit;
		}
		_twt_transfer_config_state(PHL_TWT_ACTION_FREE, &config[*para].state);
		pstatus = RTW_PHL_STATUS_SUCCESS;
		goto exit;
	} else if (type == PHL_GET_HEAD_CONFIG) {
		*ret_config = config;
		pstatus = RTW_PHL_STATUS_SUCCESS;
		goto exit;
	} else if (type == PHL_GET_NEXT_CONFIG) {
		u8 next_id = 0;
		if (*para >= twt_cfg_i->twt_cfg_num) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_operate_twt_config(): get cfg by id(%d) fail, out of range(%d)\n",
				*para, twt_cfg_i->twt_cfg_num);
			goto exit;
		}
		next_id = *para + 1;
		if (next_id == twt_cfg_i->twt_cfg_num)
			next_id = 0;
		*ret_config = &config[next_id];
		pstatus = RTW_PHL_STATUS_SUCCESS;
		goto exit;
	}
	for (i = 0; i < twt_cfg_i->twt_cfg_num; i++) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "_twt_operate_twt_config(): loop i(%d), cfg id(%d), state(%d)\n",
		i, config[i].idx, config[i].state);
		if (type == PHL_GET_NEW_CONFIG) {
			if (twt_config_state_free != config[i].state)
				continue;
			_twt_reset_config_info(phl_info, &config[i]);
			_twt_transfer_config_state(PHL_TWT_ACTION_ALLOC,
							&config[i].state);
			config[i].twt_info.twt_id = config[i].idx;
			*ret_config = &config[i];
			pstatus = RTW_PHL_STATUS_SUCCESS;
			break;
		} else if (type == PHL_GET_CONFIG_BY_ROLE) {
			if (twt_config_state_free == config[i].state)
				continue;
			if ((struct rtw_wifi_role_t *)para != config[i].role)
				continue;
			*ret_config = &config[i];
			pstatus = RTW_PHL_STATUS_SUCCESS;
			break;
		} else if (type == PHL_GET_CONFIG_BY_PARA) {
			if (twt_config_state_free == config[i].state)
				continue;
			if (!_twt_is_same_config(&config[i],
						(struct _twt_compare *)para))
				continue;
			*ret_config = &config[i];
			pstatus = RTW_PHL_STATUS_SUCCESS;
			break;
		}
	}
exit:
	_twt_dump_twt_cfg_info(twt_cfg_i);
	PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "<== _twt_operate_twt_config(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

enum rtw_phl_status _twt_sta_update(void *hal, u16 macid, u8 twt_id,
				enum rtw_phl_twt_sta_action action)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	enum rtw_hal_status hstatus = RTW_HAL_STATUS_FAILURE;

	hstatus = rtw_hal_twt_sta_update(hal, (u8)macid, twt_id, action);
	if (hstatus != RTW_HAL_STATUS_SUCCESS) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "twt sta update fail: hstatus:%d, macid:%d, twt_id:%d, action:%d\n",
			hstatus, macid, twt_id, action);
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "twt sta update ok: macid:%d, twt_id:%d, action:%d\n",
			macid, twt_id, action);
		pstatus = RTW_PHL_STATUS_SUCCESS;
	}
	return pstatus;
}

enum rtw_phl_status _twt_all_sta_update(struct phl_info_t *phl_info,
				u8 config_id, struct phl_queue *sta_queue,
				enum rtw_phl_twt_sta_action action)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	void *drv = phl_to_drvpriv(phl_info);
	_os_list *sta_list = &sta_queue->queue;
	struct rtw_twt_sta_info *psta = NULL;

	_os_spinlock(drv, &sta_queue->lock, _bh, NULL);
	phl_list_for_loop(psta, struct rtw_twt_sta_info, sta_list, list) {
		if (NULL == psta)
			break;
		pstatus = _twt_sta_update(phl_info->hal, psta->phl_sta->macid,
						config_id, action);
		if (RTW_PHL_STATUS_SUCCESS != pstatus)
			break;
	}
	_os_spinunlock(drv, &sta_queue->lock, _bh, NULL);
	return pstatus;
}

struct rtw_twt_sta_info * _twt_get_sta_info(struct phl_info_t *phl_info,
		struct phl_queue *sta_queue, struct rtw_phl_stainfo_t *phl_sta)
{
	void *drv = phl_to_drvpriv(phl_info);
	_os_list *sta_list = &sta_queue->queue;
	struct rtw_twt_sta_info *psta = NULL, *ret_sta = NULL;

	_os_spinlock(drv, &sta_queue->lock, _bh, NULL);
	phl_list_for_loop(psta, struct rtw_twt_sta_info, sta_list, list) {
		if (NULL == psta)
			break;
		if (phl_sta == psta->phl_sta) {
			ret_sta = psta;
			break;
		}
	}
	_os_spinunlock(drv, &sta_queue->lock, _bh, NULL);
	return ret_sta;
}

enum rtw_phl_status _twt_sta_enqueue(struct phl_info_t *phl_info,
		struct phl_queue *sta_q, struct rtw_twt_sta_info *psta)
{
	void *drv = phl_to_drvpriv(phl_info);

	if (!psta)
		return RTW_PHL_STATUS_FAILURE;
	_os_spinlock(drv, &sta_q->lock, _bh, NULL);
	list_add_tail(&psta->list, &sta_q->queue);
	sta_q->cnt++;
	_os_spinunlock(drv, &sta_q->lock, _bh, NULL);
	return RTW_PHL_STATUS_SUCCESS;
}

struct rtw_twt_sta_info * _twt_sta_dequeue(struct phl_info_t *phl_info,
				struct phl_queue *sta_q, u16 *cnt)
{
	struct rtw_twt_sta_info *psta = NULL;
	void *drv = phl_to_drvpriv(phl_info);

	_os_spinlock(drv, &sta_q->lock, _bh, NULL);
	if (list_empty(&sta_q->queue)) {
		psta = NULL;
	} else {
		psta = list_first_entry(&sta_q->queue,
					struct rtw_twt_sta_info, list);
		list_del(&psta->list);
		sta_q->cnt--;
		*cnt = (u16)sta_q->cnt;
	}
	_os_spinunlock(drv, &sta_q->lock, _bh, NULL);
	return psta;
}

/*
 * Delete all sta entry from queue
 * @sta_queue: twt sta Q
 */
void _twt_delete_all_sta(struct phl_info_t *phl_info,
				struct phl_queue *sta_queue)
{
	struct rtw_twt_sta_info *twt_sta;
	void *drv = phl_to_drvpriv(phl_info);
	u16 cnt;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> _twt_delete_all_sta()\n");
	do {
		twt_sta = _twt_sta_dequeue(phl_info, sta_queue, &cnt);
		if (NULL != twt_sta)
			_os_mem_free(drv, twt_sta,
					sizeof(struct rtw_twt_sta_info));
	} while (twt_sta != NULL);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== _twt_delete_all_sta()\n");
}

/*
 * Delete twt sta entry by specific sta and id from queue
 * @sta_queue: twt sta Q
 * @phl_sta: specific sta
 * @id: specific twt folw id/broadcast twt id or delete all
 * @cnt: total num of sta entery in Q
 */
enum rtw_phl_status _twt_delete_sta(struct phl_info_t *phl_info,
			struct phl_queue *sta_q,
			struct rtw_phl_stainfo_t *phl_sta, u8 id, u16 *cnt)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_twt_sta_info *twt_sta, *f_sta;
	void *drv = phl_to_drvpriv(phl_info);

	f_sta = _twt_sta_dequeue(phl_info, sta_q, cnt);
	twt_sta = f_sta;
	do {
		if (twt_sta == NULL)
			break;
		if ((phl_sta == twt_sta->phl_sta) &&
			(DELETE_ALL == id || id == twt_sta->id)) {
			_os_mem_free(drv, twt_sta,
					sizeof(struct rtw_twt_sta_info));
			pstatus = RTW_PHL_STATUS_SUCCESS;
			break;
		}
		_twt_sta_enqueue(phl_info, sta_q, twt_sta);
		twt_sta = _twt_sta_dequeue(phl_info, sta_q, cnt);
		if (NULL != twt_sta && twt_sta == f_sta) {
			_twt_sta_enqueue(phl_info, sta_q, twt_sta);
			break;
		}
	} while (true);
	return pstatus;
}

/*
 * Does sta exist in twt sta entry by specific sta and id
 * @sta_queue: twt sta Q
 * @phl_sta: specific sta
 * @id: specific twt folw id/broadcast twt id or delete all
 */
enum rtw_phl_status _twt_sta_exist(struct phl_info_t *phl_info,
			struct phl_queue *sta_q,
			struct rtw_phl_stainfo_t *phl_sta, u8 id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_twt_sta_info *twt_sta = NULL;
	void *drv = phl_to_drvpriv(phl_info);
	_os_list *q_list = &sta_q->queue;

	_os_spinlock(drv, &sta_q->lock, _bh, NULL);
	phl_list_for_loop(twt_sta, struct rtw_twt_sta_info, q_list, list) {
		if (NULL == twt_sta)
			break;
		if ((phl_sta == twt_sta->phl_sta) &&
			(DELETE_ALL == id || id == twt_sta->id)) {
			pstatus = RTW_PHL_STATUS_SUCCESS;
			break;
		}
	}
	_os_spinunlock(drv, &sta_q->lock, _bh, NULL);
	return pstatus;
}


enum rtw_phl_status _twt_add_sta(struct phl_info_t *phl_info,
			struct rtw_phl_stainfo_t *phl_sta,
			struct phl_queue *sta_q, u8 id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_twt_sta_info *twt_sta;
	void *drv = phl_to_drvpriv(phl_info);

	twt_sta = _os_mem_alloc(drv, sizeof(struct rtw_twt_sta_info));
	if (NULL == twt_sta) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "_phl_twt_add_sta(): alloc rtw_twt_sta_info failed\n");
	} else {
		twt_sta->phl_sta = phl_sta;
		twt_sta->id = id;
		_twt_sta_enqueue(phl_info, sta_q, twt_sta);
		pstatus = RTW_PHL_STATUS_SUCCESS;
	}
	return pstatus;
}

enum rtw_phl_status _twt_delete_sta_info(struct phl_info_t *phl_info,
			struct rtw_phl_stainfo_t *phl_sta,
			u8 ignore_type, enum rtw_phl_nego_type nego_type,
			u8 id, u8 *bitmap)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_twt_info *phl_twt_info = get_twt_info(phl_info);
	struct phl_twt_cfg_info *twt_cfg_i = &phl_twt_info->twt_cfg_info;
	struct phl_twt_config *config = NULL, *f_config = NULL;
	enum rtw_phl_nego_type type = nego_type;
	bool delete_error = false;
	u16 cnt;
	u8 delete_id = ignore_type ? DELETE_ALL : id;

	*bitmap = 0;
	if (RTW_PHL_MANAGE_BCAST_TWT == nego_type)
		type = RTW_PHL_BCAST_TWT;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl_info, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_delete_sta_info(): Fail to get first allocate config\n");
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "_twt_delete_sta_info(): while loop, idx(%d), state(%d), role(%p), twt_id(%d), nego_type(%d)\n",
			config->idx, config->state, config->role,
			config->twt_info.twt_id, config->twt_info.nego_type);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != phl_sta->wrole)
			goto next_cfg;
		if (false == ignore_type && config->twt_info.nego_type != type)
			goto next_cfg;
		if (RTW_PHL_STATUS_SUCCESS != _twt_sta_exist(phl_info,
							&config->twt_sta_queue,
							phl_sta, delete_id))
			goto next_cfg;
		if (RTW_PHL_STATUS_SUCCESS != _twt_sta_update(phl_info->hal,
							phl_sta->macid,
							config->twt_info.twt_id,
							TWT_STA_DEL_MACID)) {
			delete_error = true;
			goto next_cfg;
		}
		if (RTW_PHL_STATUS_SUCCESS != _twt_delete_sta(phl_info,
							&config->twt_sta_queue,
							phl_sta, delete_id,
							&cnt)) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_delete_sta_info(): Fail to delete sta from twt_sta Q, macid(0x%x), delete_id(%d)\n",
				phl_sta->macid, delete_id);
			delete_error = true;
			goto next_cfg;
		}
		if (0 == cnt)
			*bitmap |= (1 << config->twt_info.twt_id);
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_delete_sta_info(): Delete sta success, config id = %d, twt_sta_queue cnt:%d, bitmap:0x%X\n",
			config->twt_info.twt_id, cnt, *bitmap);
		if (DELETE_ALL != delete_id)
			break;
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl_info,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config)) {
			delete_error = true;
			break;
		}
	} while (config != f_config);
	if (false == delete_error)
		pstatus = RTW_PHL_STATUS_SUCCESS;
	else
		pstatus = RTW_PHL_STATUS_FAILURE;
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_delete_sta_info(): pstatus:%d, nego_type(%d), ignore_type(%d), id(%d), bitmap(0x%x), wrole(%p)\n",
		pstatus, nego_type, ignore_type, id, *bitmap, phl_sta->wrole);
	return pstatus;
}

enum rtw_phl_status
_twt_info_update(struct phl_info_t *phl_info,
                 struct phl_twt_config *config,
                 enum phl_twt_action action)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	enum rtw_hal_status hstatus = RTW_HAL_STATUS_FAILURE;
	enum rtw_phl_twt_cfg_action config_action;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> _twt_info_update()\n");

	if (PHL_TWT_ACTION_ENABLE == action) {
		config_action = TWT_CFG_ADD;
	} else if (PHL_TWT_ACTION_DISABLE == action) {
		config_action = TWT_CFG_DELETE;
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_info_update(): Unexpected action:%d\n",
			action);
		goto exit;
	}

	hstatus = rtw_hal_twt_info_update(phl_info,
	                                  config->twt_info,
	                                  config->rlink,
	                                  config_action);
	if (hstatus == RTW_HAL_STATUS_SUCCESS) {
		_twt_transfer_config_state(action, &config->state);
		pstatus = RTW_PHL_STATUS_SUCCESS;
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "_twt_info_update(): update ok\n");
	} else {
		_twt_transfer_config_state(PHL_TWT_ACTION_UP_ERROR,
								&config->state);
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_info_update(): update fail, hstatus:%d\n",
			hstatus);
	}
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<==_twt_info_update(): hstatus:%d, twt_id:%d, action:%d\n",
		pstatus, config->twt_info.twt_id, action);
	return pstatus;
}

/*
void
_twt_free_config(
	void *phl,
	struct phl_twt_config *config
)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	_twt_info_update(phl_info->hal, config, PHL_TWT_ACTION_DISABLE);
	_twt_delete_all_sta(phl_info, &config->twt_sta_queue);
	_twt_reset_config_info(config);
	_twt_transfer_config_state(PHL_TWT_ACTION_FREE, &config->state);
}
*/

bool _twt_exist_same_twt_config(struct phl_info_t *phl,
		struct phl_twt_cfg_info *twt_cfg_i, struct rtw_wifi_role_t *role,
		struct rtw_phl_twt_setup_info setup_info,
		struct phl_twt_config **ret_config)
{
	bool exist = false;
	struct _twt_compare compare_info = {0};
	struct phl_twt_config *config = NULL;

	compare_info.role = role;
	compare_info.twt_setup = setup_info;
	if (RTW_PHL_STATUS_SUCCESS == _twt_operate_twt_config(phl, twt_cfg_i,
		PHL_GET_CONFIG_BY_PARA, (u8 *)&compare_info, &config)) {
		*ret_config = config;
		exist = true;
	}
	return exist;
}

/*
u8
_twt_get_new_config_entry(
	struct phl_info_t *phl,
	struct phl_queue *twt_queue,
	struct phl_twt_config **ret_config
)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_twt_config *config = NULL;
	u8 bget = false;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> _twt_get_new_config_entry()\n");
	if (RTW_PHL_STATUS_SUCCESS == _twt_operate_twt_config(phl, twt_queue,
				PHL_GET_NEW_CONFIG, NULL, &config)) {
		*ret_config = config;
		bget = true;
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== _twt_get_new_config_entry(): bget:%d\n",
		bget);
	return bget;
}
*/

bool _twt_new_config_is_available(struct phl_info_t *phl_i)
{
	struct phl_twt_info *phl_twt_info = get_twt_info(phl_i);
	struct phl_twt_cfg_info *twt_cfg_i = &phl_twt_info->twt_cfg_info;
	struct phl_twt_config *config = NULL;
	u8 available = false;

	if (RTW_PHL_STATUS_SUCCESS == _twt_operate_twt_config(phl_i, twt_cfg_i,
				PHL_GET_NEW_CONFIG, NULL, &config)) {
		_twt_operate_twt_config(phl_i, twt_cfg_i, PHL_FREE_CONFIG,
					&config->twt_info.twt_id, NULL);
		available = true;
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_new_config_is_available(): bavailable:%d\n", available);
	return available;
}

/*
 * Whether the twt flow id of sta exist in any twt config entry.
 * @phl_sta: the specific sta
 * @role: specific role for search twt config entry
 * @id: if nego_t is RTW_PHL_INDIV_TWT : twt flow id, if nego_t is RTW_PHL_BCAST_TWT/RTW_PHL_MANAGE_BCAST_TWT: BTWT ID
 * Note: for sta mode.
*/
u8 _twt_id_exist(void *phl, struct rtw_phl_stainfo_t *phl_sta,
			struct rtw_wifi_role_t *role, u8 id,
			enum rtw_phl_nego_type nego_type)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = get_twt_info(phl_info);
	struct phl_twt_cfg_info *twt_cfg_i = &phl_twt_info->twt_cfg_info;
	struct phl_twt_config *config = NULL, *f_config = NULL;
	struct rtw_twt_sta_info *twt_sta = NULL;
	bool exist = false;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: macid(%d), id(%d), nego_type(%d)\n",
		__FUNCTION__, phl_sta->macid, id, nego_type);
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Fail to get first allocate config\n",
			__FUNCTION__);
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "%s: while loop\n", __FUNCTION__);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != phl_sta->wrole ||
			nego_type != config->twt_info.nego_type)
			goto next_cfg;
		twt_sta = _twt_get_sta_info(phl_info, &config->twt_sta_queue,
						phl_sta);
		if (NULL != twt_sta && id == twt_sta->id) {
			exist = true;
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: exist the twt_flow_id:%d\n",
				__FUNCTION__, id);
			break;
		}
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config))
			break;
	} while(config != f_config);
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: twt flow id:%d, bexist:%d\n",
		__FUNCTION__, id, exist);
	return exist;
}

enum rtw_phl_status _twt_accept_bcast_by_sta(struct phl_info_t *phl,
			struct rtw_phl_twt_setup_info *setup_info,
			struct rtw_phl_stainfo_t *sta, u8 *config_id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_element *twt_ele = &setup_info->twt_element;
	struct rtw_phl_twt_control *twt_ctrl = &twt_ele->twt_ctrl;
	struct rtw_phl_bcast_twt_para_set *para = &twt_ele->info.b_twt_para_set[0];
	struct rtw_phl_btwt_i *btwt_i = &para->btwt_i;
	u8 bitmap = 0, id = 0, i = 0;
	u32 tsf_h = 0, tsf_l = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> %s\n",
		__FUNCTION__);
	/* Append high 6byte of tsf, only 2byte of tsf in twt ele. */
	if (RTW_HAL_STATUS_SUCCESS != rtw_hal_get_tsf(phl->hal,
							sta->rlink->hw_band,
							sta->rlink->hw_port,
							&tsf_h, &tsf_l)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Failed to Get tsf, hw_band(%d), hw_port(%d)\n",
			__FUNCTION__, sta->rlink->hw_band, sta->rlink->hw_port);
		goto _exit;
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: Original BTWT Para tsf(0x%08x %08x)\n",
		__FUNCTION__, para->target_wake_t_h, para->target_wake_t_l);
	para->target_wake_t_h = tsf_h;
	para->target_wake_t_l = (tsf_l & 0xffff0000) | para->target_wake_t_l;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: Full BTWT Para tsf(0x%08x %08x)\n",
		__FUNCTION__, para->target_wake_t_h, para->target_wake_t_l);
	if (_twt_id_exist(phl, sta, sta->wrole,
				btwt_i->btwt_id, RTW_PHL_BCAST_TWT)) {
		pstatus = _twt_delete_sta_info(phl, sta, false,
						twt_ctrl->nego_type,
						btwt_i->btwt_id, &bitmap);
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: btwt id(%d) exist, first, delete twt sta, pstatus:%d, bitmap:0x%x\n",
			__FUNCTION__, btwt_i->btwt_id, pstatus, bitmap);
		if (RTW_PHL_STATUS_SUCCESS == pstatus && bitmap != 0) {
			id = 0;
			do {
				i = ((bitmap >> id) & BIT0);
				if (i != 0) {
					bitmap &= ~(BIT(id));
					break;
				}
				id++;
			} while (true);
			pstatus = rtw_phl_twt_free_twt_config(phl, id);
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s:sta Q is empty in twt config entry(%d), we free it, pstatus:%d \n",
				__FUNCTION__, id, pstatus);
			if (bitmap !=0) {
				PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: TWT config entry bitmap(0x%x) != 0, some twt config entry not free. please check code\n",
					__FUNCTION__, bitmap);
			}
		}
	}
	pstatus = rtw_phl_twt_alloc_twt_config(phl, sta->rlink, *setup_info,
						true, &id);
	if (RTW_PHL_STATUS_SUCCESS == pstatus) {
		pstatus = rtw_phl_twt_add_sta_info(phl, sta, id,
						btwt_i->btwt_id);
		if (RTW_PHL_STATUS_SUCCESS != pstatus) {
			rtw_phl_twt_free_twt_config(phl, id);
		} else {
			*config_id = id;
		}
	}
_exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: pstatus:%d, config_id:%d\n",
		__FUNCTION__, pstatus, *config_id);
	return pstatus;
}

enum rtw_phl_status _twt_accept_indiv_by_sta(struct phl_info_t *phl,
			struct rtw_phl_twt_setup_info *setup_info,
			struct rtw_phl_stainfo_t *phl_sta, u8 *config_id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_element *twt_ele = &setup_info->twt_element;
	struct rtw_phl_twt_control *twt_ctrl = &twt_ele->twt_ctrl;
	struct rtw_phl_indiv_twt_para_set *para = &twt_ele->info.i_twt_para_set;
	struct rtw_phl_req_type_indiv *req_type = &para->req_type;
	u8 bitmap = 0, id = 0, i = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> _twt_accept_indiv_by_sta()\n");
	if (_twt_id_exist(phl, phl_sta, phl_sta->wrole,
				req_type->twt_flow_id, RTW_PHL_INDIV_TWT)) {
		pstatus = _twt_delete_sta_info(phl, phl_sta, false,
						twt_ctrl->nego_type,
						req_type->twt_flow_id, &bitmap);
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_accept_indiv_by_sta(): twt flow id(%d) exist, first, delete twt sta, pstatus:%d, bitmap:0x%x\n",
			req_type->twt_flow_id, pstatus, bitmap);
		if (RTW_PHL_STATUS_SUCCESS == pstatus && bitmap != 0) {
			id = 0;
			do {
				i = ((bitmap >> id) & BIT0);
				if (i != 0) {
					bitmap &= ~(BIT(id));
					break;
				}
				id++;
			} while (true);
			pstatus = rtw_phl_twt_free_twt_config(phl, id);
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_accept_indiv_by_sta():sta Q is empty in twt config entry(%d), we free it, pstatus:%d \n",
				id, pstatus);
			if (bitmap !=0) {
				PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "_twt_accept_indiv_by_sta():  TWT config entry bitmap(0x%x) != 0, some twt config entry not free. please check code\n",
						bitmap);
			}
		}
	}
	pstatus = rtw_phl_twt_alloc_twt_config(phl,
	                                       phl_sta->rlink,
	                                       *setup_info,
	                                       true,
	                                       &id);
	if (RTW_PHL_STATUS_SUCCESS == pstatus) {
		pstatus = rtw_phl_twt_add_sta_info(phl, phl_sta, id,
							req_type->twt_flow_id);
		if (RTW_PHL_STATUS_SUCCESS != pstatus) {
			rtw_phl_twt_free_twt_config(phl, id);
		} else {
			*config_id = id;
		}
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "_twt_accept_indiv_by_sta(): pstatus:%d, config_id:%d\n",
		pstatus, *config_id);
	return pstatus;
}

static u8
_get_next_twt_subfield_size_bit(u8 size)
{
	u8 size_bit = 0;

	if (size == 1)
		size_bit = 32;
	else if (size == 2)
		size_bit = 48;
	else if (size == 3)
		size_bit = 64;
	else
		size_bit = 0;

	return size_bit;
}

struct phl_twt_config *
_twt_get_cfg_by_id(struct phl_info_t *phl, struct rtw_phl_stainfo_t *sta,
			enum rtw_phl_nego_type nego_type, u8 id)
{
	struct phl_twt_info *p_twt_i = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL, *f_config = NULL, *tgt_cfg = NULL;
	struct rtw_twt_sta_info *twt_sta = NULL;

	if (false == _twt_valid(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt invalid\n", __FUNCTION__);
		goto exit;
	}
	p_twt_i = get_twt_info(phl);
	twt_cfg_i = &p_twt_i->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Fail to get first allocate config\n",
			__FUNCTION__);
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: while loop\n",
			__FUNCTION__);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != sta->wrole ||
		    nego_type != config->twt_info.nego_type)
			goto next_cfg;
		twt_sta = _twt_get_sta_info(phl, &config->twt_sta_queue, sta);
		if (NULL == twt_sta)
			goto next_cfg;
		if (twt_sta->id == id) {
			tgt_cfg = config;
			break;
		}
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config))
			goto exit;
	} while(config != f_config);
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: macid(%d), tgt_cfg(%p), nego_type(%d), id(%d)\n",
		__FUNCTION__, sta->macid, tgt_cfg, nego_type, id);
	return tgt_cfg;
}

/*
 * Initialize twt
 */
enum rtw_phl_status phl_twt_init(void *phl)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	void *drv = phl_to_drvpriv(phl_info);
	struct phl_twt_info *phl_twt_i = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL;
	u16 len = 0;
	u8 i = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> phl_twt_init()\n");
	if (NULL == phl_info->phl_twt_info) {
		phl_info->phl_twt_info = _os_mem_alloc(drv,
					sizeof(struct phl_twt_info));
		if (NULL == phl_info->phl_twt_info) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "phl_twt_init(): Failed to allocate phl_twt_info\n");
			goto exit;
		}
		_os_mem_set(phl_to_drvpriv(phl_info), phl_info->phl_twt_info,
				0, sizeof(struct phl_twt_info));
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "phl_twt_init(): Duplicate init1, please check code\n");
	}
	phl_twt_i = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_i->twt_cfg_info;
	if (NULL == twt_cfg_i->twt_cfg_ring) {
		twt_cfg_i->twt_cfg_num = MAX_NUM_HW_TWT_CONFIG;
		len = sizeof(struct phl_twt_config) * twt_cfg_i->twt_cfg_num;
		twt_cfg_i->twt_cfg_ring = _os_mem_alloc(drv, len);
		if (NULL == twt_cfg_i->twt_cfg_ring) {
			twt_cfg_i->twt_cfg_num = 0;
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "phl_twt_init(): Failed to allocate twt_cfg_ring\n");
			goto exit;
		}
		config = (struct phl_twt_config *)twt_cfg_i->twt_cfg_ring;
		for (i = 0; i < twt_cfg_i->twt_cfg_num; i++) {
			_os_mem_set(phl_to_drvpriv(phl_info), config, 0,
					sizeof(struct phl_twt_config));
			config->idx = i;
			pq_init(drv, &config->twt_sta_queue);
			config++;
		}
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "phl_twt_init(): Duplicate init2, please check code\n");
	}
	/* init for twt_annc_queue */
	pq_init(drv, &phl_twt_i->twt_annc_queue);
	pstatus = RTW_PHL_STATUS_SUCCESS;
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "<== phl_twt_init(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 * Deinitialize twt
 */
void phl_twt_deinit(void *phl)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	void *drv = phl_to_drvpriv(phl_info);
	struct phl_twt_info *phl_twt_i = get_twt_info(phl_info);
	struct phl_twt_config *config = NULL;
	struct _twt_announce_info *annc_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	u8 i = 0;
	u16 len = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> phl_twt_deinit()\n");
	if (NULL == phl_twt_i)
		goto exit;
	twt_cfg_i = &phl_twt_i->twt_cfg_info;
	if (NULL == twt_cfg_i->twt_cfg_ring)
		goto free_twt_info;
	config = (struct phl_twt_config *)(twt_cfg_i->twt_cfg_ring);
	for (i = 0; i < twt_cfg_i->twt_cfg_num; i++) {
		if (config->twt_sta_queue.cnt > 0) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "rtw_phl_twt_deinit(): config_id: %d, twt_sta_queue.cnt(%d) >0, force delete all\n",
				config->idx ,config->twt_sta_queue.cnt);
			_twt_delete_all_sta(phl_info, &config->twt_sta_queue);
		}
		pq_deinit(drv, &config->twt_sta_queue);
		config++;
	}
	len = sizeof(struct phl_twt_config) * twt_cfg_i->twt_cfg_num;
	_os_mem_free(drv, twt_cfg_i->twt_cfg_ring, len);
	do {
		annc_info = _twt_announce_info_dequeue(phl_info,
						&phl_twt_i->twt_annc_queue);
		if (NULL == annc_info)
			break;
		_os_mem_free(drv, annc_info, sizeof(struct _twt_announce_info));
	} while(true);
	pq_deinit(drv, &phl_twt_i->twt_annc_queue);
free_twt_info:
	_os_mem_free(drv, phl_info->phl_twt_info, sizeof(struct phl_twt_info));
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_deinit()\n");
}

/*
 * Allocate new twt config
 * @role: the user of twt config
 * @setup_info: twt setup info
 * @benable: whether to enable the twt config to fw,
 * if benable is equal to false, only allocate twt config entry
 * @id: Output the id of twt confi entry
 */
enum rtw_phl_status
rtw_phl_twt_alloc_twt_config(void *phl,
                             struct rtw_wifi_role_link_t *rlink,
                             struct rtw_phl_twt_setup_info setup_info,
                             u8 benable,
                             u8 *id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL;
	bool alloc = false;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_alloc_twt_config()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_alloc_twt_config(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_alloc_twt_config(): twt_init == false\n");
		return pstatus;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
/*	if (true == _twt_exist_same_twt_config(phl_info, twt_cfg_i, role,
						setup_info, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "[TWT]alloc from existing config, id = %d\n",
			config->twt_info.twt_id);
		alloc = true;
	} else */{
		if (RTW_PHL_STATUS_SUCCESS == _twt_operate_twt_config(phl_info,
				twt_cfg_i, PHL_GET_NEW_CONFIG, NULL, &config)) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "[TWT]alloc from new config, id = %d\n",
				config->twt_info.twt_id);
			alloc = true;
		} else {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "[TWT]fail to alloc new config\n");
			pstatus = RTW_PHL_STATUS_RESOURCE;
		}
	}
	if (true == alloc) {
		*id = config->twt_info.twt_id;
		config->role = rlink->wrole;
		config->rlink = rlink;
		_twt_fill_config_info(&config->twt_info, &setup_info);
		if (benable) {
			pstatus = _twt_info_update(phl_info->hal, config,
							PHL_TWT_ACTION_ENABLE);
			if (RTW_PHL_STATUS_SUCCESS != pstatus) {
				/*todo*/
			}
		} else {
			pstatus = RTW_PHL_STATUS_SUCCESS;
		}
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_alloc_twt_config(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 * Free twt config entry by specific config ID
 * @id: id of twt config entry
 */
enum rtw_phl_status rtw_phl_twt_free_twt_config(void *phl, u8 id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_free_twt_config()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_free_twt_config(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_free_twt_config(): twt_init == false\n");
		return pstatus;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS == _twt_operate_twt_config(phl, twt_cfg_i,
							PHL_GET_CONFIG_BY_ID,
							&id, &config)) {
		_twt_info_update(phl_info->hal, config, PHL_TWT_ACTION_DISABLE);
		_twt_delete_all_sta(phl_info, &config->twt_sta_queue);
		_twt_operate_twt_config(phl, twt_cfg_i, PHL_FREE_CONFIG, &id,
									NULL);
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "[TWT]Free twt config success, id = %d\n",
			id);
		pstatus = RTW_PHL_STATUS_SUCCESS;
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_free_twt_config(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 * Enable twt config by specific config id
 * @id: id of twt confi entry
 */
enum rtw_phl_status rtw_phl_twt_enable_twt_config(void *phl, u8 id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_enable_twt_config()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_enable_twt_config(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_enable_twt_config(): twt_init == false\n");
		return pstatus;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS == _twt_operate_twt_config(phl, twt_cfg_i,
					PHL_GET_CONFIG_BY_ID, &id, &config)) {
		pstatus = _twt_info_update(phl_info->hal, config,
							PHL_TWT_ACTION_ENABLE);
		if (RTW_PHL_STATUS_SUCCESS == pstatus) {
			_twt_all_sta_update(phl_info, id, &config->twt_sta_queue,
						TWT_STA_ADD_MACID);
		}
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_enable_twt_config(): pstatus:%d, id = %d\n",
		pstatus, id);
	return pstatus;
}

/*
 * Free all twt config by specific role
 * @role: specific role for search twt config entry
 */
enum rtw_phl_status rtw_phl_twt_free_all_twt_by_role(void *phl,
				struct rtw_wifi_role_t *role)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config;
	u8 id;
	bool free = false;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_free_all_twt_by_role()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_free_all_twt_by_role(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_free_all_twt_by_role(): twt_init == false\n");
		return pstatus;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	do {
		pstatus = _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_CONFIG_BY_ROLE, (u8 *)role, &config);
		if (RTW_PHL_STATUS_SUCCESS != pstatus)
			break;
		id = config->twt_info.twt_id;
		_twt_info_update(phl_info->hal, config, PHL_TWT_ACTION_DISABLE);
		_twt_delete_all_sta(phl_info, &config->twt_sta_queue);
		_twt_operate_twt_config(phl, twt_cfg_i, PHL_FREE_CONFIG, &id,
									NULL);
		free = true;
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "Free twt config success, id = %d\n",
			id);
	} while(true);
	if (true == free)
		pstatus = RTW_PHL_STATUS_SUCCESS;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_free_all_twt_by_role(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 * Pause all twt config by specific role
 * @role: specific role for search twt config entry
 */
enum rtw_phl_status rtw_phl_twt_disable_all_twt_by_role(void *phl,
				struct rtw_wifi_role_t *role)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL, *f_config = NULL;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_disable_all_twt_by_role()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_disable_all_twt_by_role(): twt_sup == false\n");
		goto exit;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_disable_all_twt_by_role(): twt_init == false\n");
		goto exit;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_disable_all_twt_by_role(): Fail to get first allocate config\n");
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_disable_all_twt_by_role(): while loop, twt_id:%d\n",
			config->twt_info.twt_id);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role == role)
			_twt_info_update(phl_info->hal, config,
					PHL_TWT_ACTION_DISABLE);
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config))
			goto exit;
	} while(config != f_config);
	pstatus = RTW_PHL_STATUS_SUCCESS;
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_disable_all_twt_by_role(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 * Enable all twt config by specific role
 * @role: specific role for search twt config entry
 */
enum rtw_phl_status rtw_phl_twt_enable_all_twt_by_role(void *phl,
				struct rtw_wifi_role_t *role)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL, *f_config = NULL;
	bool error = false;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_enable_all_twt_by_role()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_enable_all_twt_by_role(): twt_sup == false\n");
		goto exit;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_enable_all_twt_by_role(): twt_init == false\n");
		goto exit;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_enable_all_twt_by_role(): Fail to get first allocate config\n");
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "rtw_phl_twt_enable_all_twt_by_role(): while loop, twt_id:%d\n",
			config->twt_info.twt_id);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != role)
			goto next_cfg;
		pstatus = _twt_info_update(phl_info->hal, config,
						PHL_TWT_ACTION_ENABLE);
		if (RTW_PHL_STATUS_SUCCESS == pstatus) {
			pstatus = _twt_all_sta_update(phl_info,
							config->twt_info.twt_id,
							&config->twt_sta_queue,
							TWT_STA_ADD_MACID);
			if (RTW_PHL_STATUS_SUCCESS != pstatus) {
				error = true;
				PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "rtw_phl_twt_enable_all_twt_by_role(): Fail to update all twt sta, twt_id:%d\n",
						config->twt_info.twt_id);
			}
		} else {
			error = true;
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "rtw_phl_twt_enable_all_twt_by_role(): Fail to enable twt config, twt_id:%d\n",
				config->twt_info.twt_id);
		}
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config)) {
			error = true;
			break;
		}
	} while(config != f_config);
	if (true == error)
		pstatus = RTW_PHL_STATUS_FAILURE;
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_enable_all_twt_by_role(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 *  Add twt sta to specifi twt conig entry
 * @phl_sta: sta entry that you wnat to add in specifi twt conig entry
 * @config_id: id of target twt config entry
 * @id: twt flow id/ broadcast twt id
 */
enum rtw_phl_status rtw_phl_twt_add_sta_info(void *phl,
			struct rtw_phl_stainfo_t *phl_sta, u8 config_id, u8 id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL;
	struct phl_queue *sta_q = NULL;
	u16 cnt = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_add_sta_info()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_add_sta_info(): twt_sup == false\n");
		goto fail;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_add_sta_info(): twt_init == false\n");
		goto fail;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
							PHL_GET_CONFIG_BY_ID,
							&config_id, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "Fail to get TWT config by id(%d)\n",
			config_id);
		goto fail;
	}
	sta_q = &config->twt_sta_queue;
	if (RTW_PHL_STATUS_SUCCESS != _twt_add_sta(phl, phl_sta, sta_q, id)) {
		goto fail;
	}
	if (RTW_PHL_STATUS_SUCCESS != _twt_sta_update(phl_info->hal,
				phl_sta->macid, config_id, TWT_STA_ADD_MACID)) {
		_twt_delete_sta(phl, sta_q, phl_sta, id, &cnt);
		goto fail;
	}
	pstatus = RTW_PHL_STATUS_SUCCESS;
fail:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_add_sta_info(): pstatus:%d\n",
		pstatus);
	return pstatus;
}

/*
 *  Remove all twt sta from twt config entry by specific sta entry
 * @phl_sta: sta entry that you wnat to remove
 * @bitmap: Output the bitmap. Indicate the statue that twt sta don't exist in the twt config entry that twt sta removed from it.
 * ex: Bitmap=10: We remove the twt sta from id 1, id 3 and other id of twt config entry,
 * but after remove, there are no twt sta existing in the twt config entry of id 1 and id 3.
 * ex: Bitmap=0: We remove the twt sta, after remove, there are at least one twt sta existing in the twt config entry that twt sta removed from it.
 */
enum rtw_phl_status rtw_phl_twt_delete_all_sta_info(void *phl,
				struct rtw_phl_stainfo_t *phl_sta, u8 *bitmap)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_delete_all_sta_info()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_delete_all_sta_info(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_delete_all_sta_info(): twt_init == false\n");
		return pstatus;
	}
	pstatus = _twt_delete_sta_info(phl, phl_sta, true, 0, 0, bitmap);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_delete_all_sta_info(): pstatus:%d, bitmap:0x%x\n",
		pstatus, *bitmap);
	return pstatus;
}

/*
 * Remove twt sta when tx/rx twt teardown frame
 * @phl_sta: sta entry that you wnat to remove
 * @twt_flow: twt flow field info
 * @bitmap: Output the bitmap. Indicate the statue that twt sta don't exist in the twt config entry that twt sta removed from it.
 * ex: Bitmap=10: We remove the twt sta from id 1, id 3 and other id of twt config entry,
 * but after remove, there are no twt sta existing in the twt config entry of id 1 and id 3.
 * ex: Bitmap=0: We remove the twt sta, after remove, there are at least one twt sta existing in the twt config entry that twt sta removed from it.
 */
enum rtw_phl_status rtw_phl_twt_teardown_sta(void *phl,
			struct rtw_phl_stainfo_t *phl_sta,
			struct rtw_phl_twt_flow_field *twt_flow, u8 *bitmap)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	u8 id = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_teardown_sta()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_teardown_sta(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_teardown_sta(): twt_init == false\n");
		return pstatus;
	}
	if (RTW_PHL_INDIV_TWT == twt_flow->nego_type ||
		RTW_PHL_WAKE_TBTT_INR == twt_flow->nego_type) {
		if (twt_flow->info.twt_flow01.teardown_all)
			id = DELETE_ALL;
		else
			id = twt_flow->info.twt_flow01.twt_flow_id;
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else if (RTW_PHL_BCAST_TWT == twt_flow->nego_type ) {
		/*Todo*/
	} else if (RTW_PHL_MANAGE_BCAST_TWT == twt_flow->nego_type) {
		if (twt_flow->info.twt_flow3.teardown_all)
			id = DELETE_ALL;
		else
			id = twt_flow->info.twt_flow3.bcast_twt_id;
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "rtw_phl_twt_teardown_sta(): Unknown nego_type:%d\n",
			twt_flow->nego_type);
	}
	if (RTW_PHL_STATUS_SUCCESS == pstatus) {
		pstatus = _twt_delete_sta_info(phl, phl_sta, false,
					twt_flow->nego_type, id, bitmap);
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_teardown_sta(): pstatus:%d, twt_flow->nego_type:%d, id:%d, bitmap:0x%x\n",
		pstatus, twt_flow->nego_type, id, *bitmap);
	return pstatus;
}

/*
 * Assign new flow id for twt setup of sta.
 * @phl_sta: the specific sta
 * @role: specific role for search twt config entry
 * @id: Output: twt flow id
 * Note: for sta mode.
 */
enum rtw_phl_status rtw_phl_twt_get_new_flow_id(void *phl,
				struct rtw_phl_stainfo_t *phl_sta, u8 *id)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_RESOURCE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL, *f_config = NULL;
	struct rtw_twt_sta_info *twt_sta = NULL;
	u8 use_map = 0, unuse_map = 0;
	u8 i = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_get_new_flow_id()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_get_new_flow_id(): twt_sup == false\n");
		goto exit;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_get_new_flow_id(): twt_init == false\n");
		goto exit;
	}
	phl_twt_info = get_twt_info(phl_info);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (false == _twt_new_config_is_available(phl_info))
		goto exit;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_get_new_flow_id(): Fail to get first allocate config\n");
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_get_new_flow_id(): while loop\n");
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != phl_sta->wrole ||
			RTW_PHL_INDIV_TWT != config->twt_info.nego_type)
			goto next_cfg;
		twt_sta = _twt_get_sta_info(phl_info, &config->twt_sta_queue,
						phl_sta);
		if (NULL != twt_sta) {
			use_map |= (1 << twt_sta->id);
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_get_new_flow_id(): config_ID:%d, get match sta, twt_sta->id:%d\n",
				config->twt_info.twt_id, twt_sta->id);
		}
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config))
			goto exit;
	} while(config != f_config);
	unuse_map = (~use_map) & 0xFF;
	i = 0;
	while ((unuse_map >> i) > 0) {
		if ((unuse_map >> i) & BIT0) {
			*id = i;
			pstatus = RTW_PHL_STATUS_SUCCESS;
			break;
		}
		i++;
	}
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_get_new_flow_id(): pstatus:%d, use_map:%d, unuse_map:%d, new_flow_id:%d\n",
		pstatus, use_map, unuse_map, *id);
	return pstatus;
}

/*
 * if nego_type is RTW_PHL_INDIV_TWT, return flow id
 * if nego_type is RTW_PHL_BCAST_TWT or RTW_PHL_MANAGE_BCAST_TWT, return btwt id
 */
enum rtw_phl_status
rtw_phl_twt_get_all_id(void *phl, struct rtw_phl_stainfo_t *phl_sta,
			enum rtw_phl_nego_type nego_type, u8 *used_map)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_RESOURCE;
	struct phl_info_t *phl_i = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL, *f_config = NULL;
	struct rtw_twt_sta_info *twt_sta = NULL;

	if (false == _twt_valid(phl_i)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt invalid\n", __FUNCTION__);
		goto exit;
	}
	*used_map = 0;
	phl_twt_info = get_twt_info(phl_i);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Fail to get first allocate config\n",
			__FUNCTION__);
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_DEBUG_, "%s: while loop\n",
			__FUNCTION__);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != phl_sta->wrole ||
			nego_type != config->twt_info.nego_type)
			goto next_cfg;
		twt_sta = _twt_get_sta_info(phl_i, &config->twt_sta_queue,
						phl_sta);
		if (NULL != twt_sta) {
			(*used_map) |= (1 << twt_sta->id);
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: config_ID:%d, get match sta, twt_sta->id:%d\n",
				__FUNCTION__, config->twt_info.twt_id, twt_sta->id);
		}
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config))
			goto exit;
	} while(config != f_config);
	sts = RTW_PHL_STATUS_SUCCESS;
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: sts(%d), nego_type(%d), used_map(0x%x)\n",
		__FUNCTION__, sts, nego_type, *used_map);
	return sts;
}

u8
rtw_phl_twt_get_cfg_id(void *phl, struct rtw_phl_stainfo_t *sta,
			enum rtw_phl_nego_type nego_type, u8 id, u8 *cfg_id)
{
	u8 ret = false;
	struct phl_info_t *phl_i = (struct phl_info_t *)phl;
	struct phl_twt_info *p_twt_i = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL, *f_config = NULL, *tgt_cfg = NULL;
	struct rtw_twt_sta_info *twt_sta = NULL;

	if (false == _twt_valid(phl_i)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt invalid\n", __FUNCTION__);
		goto exit;
	}
	p_twt_i = get_twt_info(phl_i);
	twt_cfg_i = &p_twt_i->twt_cfg_info;
	if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl_i, twt_cfg_i,
				PHL_GET_HEAD_CONFIG, NULL, &config)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Fail to get first allocate config\n",
			__FUNCTION__);
		goto exit;
	}
	f_config = config;
	do {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: while loop\n",
			__FUNCTION__);
		if (twt_config_state_free == config->state)
			goto next_cfg;
		if (config->role != sta->wrole ||
		    nego_type != config->twt_info.nego_type)
			goto next_cfg;
		twt_sta = _twt_get_sta_info(phl_i, &config->twt_sta_queue, sta);
		if (NULL == twt_sta)
			goto next_cfg;
		if (twt_sta->id == id) {
			tgt_cfg = config;
			break;
		}
next_cfg:
		if (RTW_PHL_STATUS_SUCCESS != _twt_operate_twt_config(phl_i,
						twt_cfg_i, PHL_GET_NEXT_CONFIG,
						(u8 *)&config->idx, &config))
			goto exit;
	} while(config != f_config);
exit:
	if (tgt_cfg) {
		*cfg_id = tgt_cfg->idx;
		ret = true;
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: macid(%d), cfg_id(%d), nego_type(%d), flow/b id(%d)\n",
			__FUNCTION__, sta->macid, *cfg_id, nego_type, id);
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: Failed!! macid(%d), nego_type(%d), flow/b id(%d)\n",
			__FUNCTION__, sta->macid, nego_type, id);
	}
	return ret;
}

static void _phl_twt_get_target_wake_time_done(void *priv, u8 *param,
				u32 param_len, enum rtw_phl_status sts)
{
	if (param) {
		_os_kmem_free(priv, param, param_len);
		param = NULL;
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s\n", __func__);
	}
}

enum rtw_phl_status phl_twt_get_target_wake_time(struct phl_info_t *phl,
						u8 *param)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	enum rtw_hal_status hstatus = RTW_HAL_STATUS_FAILURE;
	struct rtw_phl_twt_get_twt_i *get_twt_i =
					(struct rtw_phl_twt_get_twt_i *)param;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_twt_cfg_info *twt_cfg_i = NULL;
	struct phl_twt_config *config = NULL;
	u32 c_tsf_l = 0, c_tsf_h = 0, intvl = 0;
	u64 cur_tsf = 0, tgt_tsf = 0, ref_tsf = 0, dif_tsf = 0;

	if (false == twt_sup(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_sup == false\n",
			__func__);
		goto exit;
	}
	if (false == twt_init(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_init == false\n",
			__func__);
		goto exit;
	}
	phl_twt_info = get_twt_info(phl);
	twt_cfg_i = &phl_twt_info->twt_cfg_info;
	hstatus = rtw_hal_get_tsf(phl->hal,
			get_twt_i->wrole->rlink[RTW_RLINK_PRIMARY].hw_band,
			get_twt_i->wrole->rlink[RTW_RLINK_PRIMARY].hw_port,
			&c_tsf_h, &c_tsf_l);
	if (RTW_HAL_STATUS_FAILURE == hstatus) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "%s: Fail to get tsf, hstatus:%d, port:%d\n",
			__func__, hstatus, get_twt_i->wrole->rlink[RTW_RLINK_PRIMARY].hw_port);
		goto exit;
	}
	cur_tsf = c_tsf_h;
	cur_tsf = cur_tsf << 32;
	cur_tsf |= c_tsf_l;
	if (IGNORE_CFG_ID == get_twt_i->id) {
		tgt_tsf = _os_add64(cur_tsf, get_twt_i->offset * 1000);
	} else {
		pstatus = _twt_operate_twt_config(phl, twt_cfg_i,
				PHL_GET_CONFIG_BY_ID, &get_twt_i->id, &config);
		if (RTW_PHL_STATUS_SUCCESS != pstatus) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "%s: Fail to get twt entry by id, pstatus:%d, id:%d\n",
				__func__, pstatus, get_twt_i->id);
			goto exit;
		}
		ref_tsf = config->twt_info.target_wake_time_h;
		ref_tsf = ref_tsf << 32;
		ref_tsf |= config->twt_info.target_wake_time_l;
		intvl = _twt_calc_intvl(config->twt_info.twt_wake_int_exp,
					config->twt_info.twt_wake_int_mantissa);
		tgt_tsf = _os_add64(cur_tsf, get_twt_i->offset * 1000);
		dif_tsf = _os_minus64(tgt_tsf, ref_tsf);
		tgt_tsf = _os_minus64(tgt_tsf, _os_modular64(dif_tsf, intvl));
		tgt_tsf = _os_add64(tgt_tsf, intvl);
	}
	*(get_twt_i->tsf_h) = (u32)(tgt_tsf >> 32);
	*(get_twt_i->tsf_l) = (u32)(tgt_tsf);
	pstatus = RTW_PHL_STATUS_SUCCESS;
exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: pstatus(%d), port:%d, twt_id:%d, offset:0x%08x, tsf_h: 0x%08X, tsf_l: 0x%08X\n",
		__func__, pstatus, get_twt_i->wrole->rlink[RTW_RLINK_PRIMARY].hw_port, get_twt_i->id,
		get_twt_i->offset, *(get_twt_i->tsf_h), *(get_twt_i->tsf_l));

	return pstatus;
}

/*
 * get target wake time, only handle PHL_CMD_DIRECTLY or PHL_CMD_WAIT of cmd_type
 * @wrole: Specific wrole
 * @id: reference id of twt configuration
 * @offset: unit: ms. An amount of time that you will start TWT from now
 * @tsf_h: return high 4-byte value of target wake time
 * @tsf_l: return low 4-byte value of target wake time
 */
enum rtw_phl_status
rtw_phl_twt_get_target_wake_time(void *phl,
			struct rtw_phl_twt_get_twt_i *get_twt_i,
			enum phl_cmd_type cmd_type, u32 cmd_timeout)
{
	enum rtw_phl_status psts = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_i = (struct phl_info_t *)phl;
	void *drv_priv = phl_to_drvpriv(phl_i);
	struct rtw_phl_twt_get_twt_i *para = NULL;
	u32 para_len = 0;

	if (PHL_CMD_DIRECTLY != cmd_type && PHL_CMD_WAIT != cmd_type) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s error cmd_type(%d) \n",
			__func__, cmd_type);
		goto _exit;
	}
	para_len = sizeof(struct rtw_phl_twt_get_twt_i);
	para = _os_kmem_alloc(drv_priv, para_len);
	if (para == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s - alloc param failed!\n",
			__func__);
		goto _exit;
	}
	para->wrole = get_twt_i->wrole;
	para->id = get_twt_i->id;
	para->offset = get_twt_i->offset;
	para->tsf_h = get_twt_i->tsf_h;
	para->tsf_l = get_twt_i->tsf_l;
	if (cmd_type == PHL_CMD_DIRECTLY) {
		psts = phl_twt_get_target_wake_time(phl_i, (u8 *)para);
		_os_kmem_free(drv_priv, para, para_len);
		goto _exit;
	}
	psts = phl_cmd_enqueue(phl_i,
				para->wrole->rlink[RTW_RLINK_PRIMARY].hw_band,
				MSG_EVT_TWT_GET_TWT,
				(u8 *)para,
				para_len,
				_phl_twt_get_target_wake_time_done,
				cmd_type,
				cmd_timeout);
	if (is_cmd_failure(psts)) {
		/* Send cmd success, but wait cmd fail*/
		psts = RTW_PHL_STATUS_FAILURE;
	} else if (psts != RTW_PHL_STATUS_SUCCESS) {
		/* Send cmd fail */
		_os_kmem_free(drv_priv, para, para_len);
		psts = RTW_PHL_STATUS_FAILURE;
	}
_exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s psts(%d)\n", __func__, psts);
	return psts;
}

/*
 * Fill twt element
 * @twt_ele: twt element info
 * @buf: fill memory
 * @len: the length of twt element
 */
enum rtw_phl_status rtw_phl_twt_fill_twt_element(
			struct rtw_phl_twt_element *twt_ele, u8 *buf, u8 *len)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	u8 twt_para_length = 0, idx = 0;
	struct rtw_phl_twt_control *twt_ctrl = NULL;
	struct rtw_phl_bcast_twt_para_set *btwt_para = NULL;

	if (twt_ele == NULL || buf == NULL || len == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_ele or buf or len = NULL\n",
			__FUNCTION__);
		return pstatus;
	}
	twt_ctrl = &twt_ele->twt_ctrl;
	_dump_ctrl_i(twt_ctrl, __FUNCTION__);
	*len = 0;
/*Control filed*/
	SET_TWT_CONTROL_NDP_PAGING_INDICATOR(buf, twt_ctrl->ndp_paging_indic);
	SET_TWT_CONTROL_RESPONDER_PM_MODE(buf, twt_ctrl->responder_pm_mode);
	SET_TWT_CONTROL_NEGOTIATION_TYPE(buf, twt_ctrl->nego_type);
	SET_TWT_CONTROL_TWT_INFORMATION_FRAME_DISABLE(buf,
					twt_ctrl->twt_info_frame_disable);
	SET_TWT_CONTROL_WAKE_DURATION_UNIT(buf, twt_ctrl->wake_dur_unit);
	*len += CONTROL_LENGTH;
/*TWT Parameter Information*/
	if (RTW_PHL_INDIV_TWT == twt_ctrl->nego_type) {
		pstatus = _twt_fill_individual_twt_para_set(
					&twt_ele->info.i_twt_para_set,
					twt_ctrl->ndp_paging_indic,
					buf + *len, &twt_para_length);
		*len += twt_para_length;
	} else if (RTW_PHL_BCAST_TWT == twt_ctrl->nego_type ||
		   RTW_PHL_MANAGE_BCAST_TWT == twt_ctrl->nego_type) {
		if (twt_ele->num_btwt_para > MAX_BTWT_PARA_SET) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_ele->num_btwt_para(%d) > MAX_BTWT_PARA_SET(%d)\n",
				__FUNCTION__, twt_ele->num_btwt_para,
				MAX_BTWT_PARA_SET);
			goto _exit;
		}
		for (idx = 0; idx < twt_ele->num_btwt_para; idx++) {
			btwt_para = &twt_ele->info.b_twt_para_set[idx];
			if ((idx + 1) == twt_ele->num_btwt_para) {
				btwt_para->req_type.lst_bc_para_set = 1;
			} else {
				btwt_para->req_type.lst_bc_para_set = 0;
			}
			pstatus = _twt_fill_btwt_para_set(btwt_para,
							buf + *len,
							&twt_para_length);
			*len += twt_para_length;
		}
	} else {
		/*todo*/
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: not support, todo, twt_ctrl->nego_type(%d)\n",
			__FUNCTION__, twt_ctrl->nego_type);
	}
_exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s()\n", __FUNCTION__);
	return pstatus;
}

/*
 * Fill twt flow field of TWT teardown frame
 * @twt_flow: twt flow field info
 * @buf: fill memory
 * @length: the length of twt flow field
 */
enum rtw_phl_status rtw_phl_twt_fill_flow_field(
		struct rtw_phl_twt_flow_field *twt_flow, u8 *buf, u16 *length)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_fill_flow_field()\n");
	if (twt_flow == NULL || buf == NULL || length == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_fill_flow_field(): twt_flow or buf or length = NULL\n");
		return pstatus;
	}
	*length = 0;
	if (RTW_PHL_INDIV_TWT == twt_flow->nego_type ||
		RTW_PHL_WAKE_TBTT_INR == twt_flow->nego_type) {
		struct rtw_phl_twt_flow_type01 *flow_info =
						&twt_flow->info.twt_flow01;
		SET_TWT_FLOW_ID(buf, flow_info->twt_flow_id);
		SET_NEGOTIATION_TYPE(buf, twt_flow->nego_type);
		SET_TEARDOWN_ALL_TWT(buf, flow_info->teardown_all);
		*length = TWT_FLOW_FIELD_LENGTH;
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else if (RTW_PHL_BCAST_TWT  == twt_flow->nego_type) {
		SET_NEGOTIATION_TYPE(buf, twt_flow->nego_type);
		*length = TWT_FLOW_FIELD_LENGTH;
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else if (RTW_PHL_MANAGE_BCAST_TWT == twt_flow->nego_type) {
		struct rtw_phl_twt_flow_type3 *flow_info =
						&twt_flow->info.twt_flow3;
		SET_BROADCAST_TWT_ID(buf, flow_info->bcast_twt_id);
		SET_NEGOTIATION_TYPE(buf, twt_flow->nego_type);
		SET_TEARDOWN_ALL_TWT(buf, flow_info->teardown_all);
		*length = TWT_FLOW_FIELD_LENGTH;
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_fill_flow_field(): Unknown type(%d)\n",
			twt_flow->nego_type);
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_fill_flow_field()\n");
	return pstatus;
}

enum rtw_phl_status rtw_phl_twt_fill_info_field(
	struct rtw_phl_twt_info_f *twt_info, u8 *buf, u16 *length)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_FAILURE;

	_dump_twt_info(twt_info, __FUNCTION__);
	*length = 0;
	SET_TWT_INFO_FLOW_ID(buf, twt_info->twt_flow_id);
	SET_TWT_INFO_RSP_REQ(buf, twt_info->rsp_req);
	SET_TWT_INFO_ALL_TWT(buf, twt_info->all_twt);
	SET_TWT_INFO_NEXT_TWT_REQ(buf, twt_info->next_twt_req);
	if (twt_info->next_twt_size) {
		u8 size_bit =
			_get_next_twt_subfield_size_bit(twt_info->next_twt_size);

		SET_TWT_INFO_NEXT_TWT_SIZE(buf, twt_info->next_twt_size);
		SET_TWT_INFO_NEXT_TWT(buf, size_bit, twt_info->next_twt);
		*length = TWT_INFO_FIELD_BASIC_LENGTH + (size_bit / 8);
	} else {
		*length = TWT_INFO_FIELD_BASIC_LENGTH;
	}
	sts = RTW_PHL_STATUS_SUCCESS;
	return sts;
}

/*
 * Parse twt element from pkt
 * @twt_ele: the address of twt elemant
 * @length: length of pkt
 * @twt_element: Parse info
 */
enum rtw_phl_status rtw_phl_twt_parse_element(u8 *twt_ele, u16 length,
				struct rtw_phl_twt_element *twt_element)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_control *twt_ctrl = NULL;
	u8 ele_len = 0, ele_id = 0, para_len = 0;
	u8 r_len = 0, plen = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> %s()\n", __FUNCTION__);
	if (twt_ele == NULL || twt_element == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_ele or twt_element = NULL\n",
			__FUNCTION__);
		return sts;
	}
	twt_ctrl = &twt_element->twt_ctrl;
	ele_id = GET_ELE_ID(twt_ele);
	r_len = ELEM_ID_LEN;
	ele_len = GET_ELE_LEN(twt_ele + r_len);
	r_len += ELEM_LEN_LEN;
	para_len = ele_len - CONTROL_LENGTH;
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: ele_id(%d), ele_len(%d), length(%d), para_len(%d)\n",
		__FUNCTION__, ele_id, ele_len, length, para_len);
	twt_ctrl->ndp_paging_indic =
			GET_TWT_CONTROL_NDP_PAGING_INDICATOR(twt_ele + r_len);
	twt_ctrl->responder_pm_mode =
			GET_TWT_CONTROL_RESPONDER_PM_MODE(twt_ele + r_len);
	twt_ctrl->nego_type = GET_TWT_CONTROL_NEGOTIATION_TYPE(twt_ele + r_len);
	twt_ctrl->twt_info_frame_disable =
		GET_TWT_CONTROL_TWT_INFORMATION_FRAME_DISABLE(twt_ele + r_len);
	twt_ctrl->wake_dur_unit =
			GET_TWT_CONTROL_WAKE_DURATION_UNIT(twt_ele + r_len);
	r_len += CONTROL_LENGTH;
	_dump_ctrl_i(twt_ctrl, __FUNCTION__);
	if (RTW_PHL_INDIV_TWT == twt_ctrl->nego_type) {
		sts = _twt_parse_individual_twt_para(twt_ele, length,
							twt_element);
	} else if (RTW_PHL_BCAST_TWT == twt_ctrl->nego_type ||
		   RTW_PHL_MANAGE_BCAST_TWT == twt_ctrl->nego_type) {
		sts = _twt_parse_all_btwt_para(twt_ele + r_len, para_len, twt_element, &plen);
		r_len += plen;
	} else {
		/*todo*/
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: not support, todo, twt_ctrl->nego_type(%d)\n",
			__FUNCTION__, twt_ctrl->nego_type);
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: pstatus(%d)\n",
		__FUNCTION__, sts);
	return sts;
}

/*
 * Parse twt setup info from pkt
 * @pkt: the address of Category of twt setup info frame
 * @length: length of pkt
 * @twt_setup_info: Parse info
 */
enum rtw_phl_status rtw_phl_twt_parse_setup_info(u8 *pkt, u16 length,
				struct rtw_phl_twt_setup_info *setup_info)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	u8 *twt_ele = NULL;
	u16 ele_length = length - TOKEN_OFFSET- TOKEN_LENGTH;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_parse_setup_info()\n");
	if (pkt == NULL || setup_info == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_parse_setup_info(): pkt or setup_info = NULL\n");
		return pstatus;
	}
	twt_ele = pkt + TOKEN_OFFSET + TOKEN_LENGTH;
	setup_info->dialog_token = GET_DIALOG_TOKEN(pkt + TOKEN_OFFSET);
	pstatus = rtw_phl_twt_parse_element(twt_ele, ele_length,
						&setup_info->twt_element);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_parse_setup_info(): pstatus(%d)\n",
		pstatus);
	return pstatus;
}

/*
 * Parse twt twt flow field from twt teardown frame
 * @pkt: the address of twt flow field
 * @length: length of pkt
 * @twt_flow_info: Parse info
 */
enum rtw_phl_status rtw_phl_twt_parse_flow_field(u8 *pkt, u16 length,
				struct rtw_phl_twt_flow_field *twt_flow)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_parse_flow_field()\n");
	if (pkt == NULL || twt_flow == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_parse_flow_field(): pkt or twt_flow = NULL\n");
		return pstatus;
	}
	twt_flow->nego_type = GET_NEGOTIATION_TYPE(pkt);
	if (RTW_PHL_INDIV_TWT == twt_flow->nego_type ||
		RTW_PHL_WAKE_TBTT_INR == twt_flow->nego_type) {
		struct rtw_phl_twt_flow_type01 *flow_info =
						&twt_flow->info.twt_flow01;
		flow_info->twt_flow_id = GET_TWT_FLOW_ID(pkt);
		flow_info->teardown_all = GET_TEARDOWN_ALL_TWT(pkt);
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_parse_flow_field(): nego_type:%d, twt_flow_id:%d, teardown_all:%d\n",
			twt_flow->nego_type, flow_info->twt_flow_id,
			flow_info->teardown_all);
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else if (RTW_PHL_BCAST_TWT == twt_flow->nego_type) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_parse_flow_field(): not support, todo, twt_flow->nego_type(%d)\n",
			twt_flow->nego_type);
	} else if (RTW_PHL_MANAGE_BCAST_TWT == twt_flow->nego_type) {
		struct rtw_phl_twt_flow_type3 *flow_info =
						&twt_flow->info.twt_flow3;
		flow_info->bcast_twt_id = GET_BROADCAST_TWT_ID(pkt);
		flow_info->teardown_all = GET_TEARDOWN_ALL_TWT(pkt);
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_parse_flow_field(): nego_type:%d, bcast_twt_id:%d, teardown_all:%d\n",
			twt_flow->nego_type, flow_info->bcast_twt_id,
			flow_info->teardown_all);
		pstatus = RTW_PHL_STATUS_SUCCESS;
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_parse_flow_field(): Unknown type, twt_flow->nego_type(%d)\n",
			twt_flow->nego_type);
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_parse_flow_field(): pstatus(%d)\n",
		pstatus);
	return pstatus;
}

enum rtw_phl_status rtw_phl_twt_parse_info_field(u8 *pkt, u16 length,
				struct rtw_phl_twt_info_f *twt_info)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_FAILURE;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: \n", __FUNCTION__);

	if (length < TWT_INFO_FIELD_BASIC_LENGTH) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "%s: error buffer length(%d) < %d\n",
			__FUNCTION__, length, TWT_INFO_FIELD_BASIC_LENGTH);
		goto _exit;
	}
	twt_info->twt_flow_id = GET_TWT_INFO_FLOW_ID(pkt);
	twt_info->rsp_req = GET_TWT_INFO_RSP_REQ(pkt);
	twt_info->next_twt_req = GET_TWT_INFO_NEXT_TWT_REQ(pkt);
	twt_info->next_twt_size = GET_TWT_INFO_NEXT_TWT_SIZE(pkt);
	twt_info->all_twt = GET_TWT_INFO_ALL_TWT(pkt);
	if (twt_info->next_twt_size) {
		u8 size_bit = _get_next_twt_subfield_size_bit(
						twt_info->next_twt_size);

		if (length < (TWT_INFO_FIELD_BASIC_LENGTH + (size_bit / 8))) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_WARNING_, "%s: next_twt_size(%d), error buffer length(%d) < %d\n",
				__FUNCTION__,
				twt_info->next_twt_size,
				length,
				(TWT_INFO_FIELD_BASIC_LENGTH + (size_bit / 8)));
			goto _exit;
		}
		GET_TWT_INFO_NEXT_TWT(pkt, size_bit, &twt_info->next_twt);
	}
_exit:
	sts = RTW_PHL_STATUS_SUCCESS;
	return sts;
}

/*
 * Tell fw which macid is announced in awake state
 * @macid: macid of sta that is in awake state
 */
enum rtw_phl_status rtw_phl_twt_sta_announce_to_fw(void *phl,
				u16 macid)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_twt_info *phl_twt_info = NULL;
	struct phl_queue *annc_q = NULL;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> rtw_phl_twt_sta_announce_to_fw()\n");
	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_sta_announce_to_fw(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_sta_announce_to_fw(): twt_init == false\n");
		return pstatus;
	}
	phl_twt_info = get_twt_info(phl_info);
	annc_q = &phl_twt_info->twt_annc_queue;
	pstatus = _twt_sta_announce(phl_info, annc_q, macid);
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== rtw_phl_twt_sta_announce_to_fw(): pstatus:%d, macid: %d\n",
		pstatus, macid);
	return pstatus;
}

#if 0
/*
 * Handle twt c2h
 * @c: c2h content
 */
enum rtw_phl_status rtw_phl_twt_handle_c2h(void *phl_com, void *c)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_com_t *phl_com_info= (struct rtw_phl_com_t *)phl_com;
	struct phl_info_t *phl_info = (struct phl_info_t*)phl_com_info->phl_priv;
	struct rtw_c2h_info *c2h = (struct rtw_c2h_info *)c;

	if (false == twt_sup(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_handle_c2h(): twt_sup == false\n");
		return pstatus;
	}
	if (false == twt_init(phl_info)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "rtw_phl_twt_handle_c2h(): twt_init == false\n");
		return pstatus;
	}
	if (C2H_FUN_WAIT_ANNC == c2h->c2h_func) {
		pstatus = _twt_handle_c2h_wait_annc(phl_info, c2h->content);
	}
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "rtw_phl_twt_handle_c2h(): pstatus:%d cat:%d, class:%d, func:%d, len:%d, content:0x%x\n",
		pstatus, c2h->c2h_cat, c2h->c2h_class, c2h->c2h_func,
		c2h->content_len, *(c2h->content));
	return pstatus;
}
#endif

static void _phl_twt_sta_accept_twt_done(void *priv, u8 *param,
				u32 param_len, enum rtw_phl_status sts)
{
	if (param) {
		struct rtw_phl_twt_sta_accept_i *accept_i =
				(struct rtw_phl_twt_sta_accept_i *)param;

		if (accept_i->accept_done) {
			accept_i->accept_done(priv, accept_i->sta,
						&accept_i->setup_info, sts);
		}
		_os_kmem_free(priv, param, param_len);
		param = NULL;
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s\n", __func__);
	}
}

enum rtw_phl_status phl_twt_accept_for_sta_mode(struct phl_info_t *phl,
			u8 *param)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_sta_accept_i *accept_i =
				(struct rtw_phl_twt_sta_accept_i *)param;
	struct rtw_phl_stainfo_t *sta = accept_i->sta;
	struct rtw_phl_twt_setup_info *setup_i = &accept_i->setup_info;
	struct rtw_phl_twt_element *element = &setup_i->twt_element;
	struct rtw_phl_twt_control *twt_ctrl =&element->twt_ctrl;
	struct phl_twt_info *phl_twt_info = get_twt_info(phl);
	struct rtw_pkt_ofld_null_info null_info = {0};
	void *d = phl_to_drvpriv(phl);
	u8 cfg_id = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> %s()\n",
		__func__);
	if (false == _twt_valid(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt invalid\n", __FUNCTION__);
		return pstatus;
	}
	if (RTW_PHL_INDIV_TWT == twt_ctrl->nego_type) {
		pstatus = _twt_accept_indiv_by_sta(phl, setup_i, sta, &cfg_id);
	} else if (RTW_PHL_BCAST_TWT == twt_ctrl->nego_type ||
		   RTW_PHL_MANAGE_BCAST_TWT == twt_ctrl->nego_type) {
		pstatus = _twt_accept_bcast_by_sta(phl, setup_i, sta, &cfg_id);
	} else {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Unimplemented twt_ctrl->nego_type(%d)\n",
			__func__, twt_ctrl->nego_type);
	}

	_os_mem_cpy(d, &(null_info.a1[0]), &(sta->mac_addr[0]),
			MAC_ADDRESS_LENGTH);

	_os_mem_cpy(d,&(null_info.a2[0]), &(sta->wrole->mac_addr[0]),
			MAC_ADDRESS_LENGTH);

	_os_mem_cpy(d, &(null_info.a3[0]), &(sta->mac_addr[0]),
			MAC_ADDRESS_LENGTH);

	rtw_phl_pkt_ofld_request(phl, sta->macid, PKT_TYPE_QOS_NULL,
		&phl_twt_info->qos_null_pkt_token, &null_info, __func__);

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: pstatus:%d, config_id:%d\n",
		__func__, pstatus, cfg_id);
	return pstatus;
}

/*
 * Handle sta to config twt when sta accept the twt agreement
 * @phl_sta: sta entry that you wnat to config twt
 * @setup_info: twt setup info
 * @id: Output the id of twt confi entry
 * Note: for sta mode
 */
enum rtw_phl_status rtw_phl_twt_accept_for_sta_mode(void *phl,
				struct rtw_phl_twt_sta_accept_i *accept_i)
{
	enum rtw_phl_status psts = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	void *drv_priv = phl_to_drvpriv(phl_info);
	struct rtw_phl_twt_sta_accept_i *para = NULL;
	u32 para_len = 0;

	para_len = sizeof(struct rtw_phl_twt_sta_accept_i);
	para = _os_kmem_alloc(drv_priv, para_len);
	if (para == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s - alloc param failed!\n",
			__func__);
		goto _exit;
	}
	para->accept_done = accept_i->accept_done;
	para->sta = accept_i->sta;
	_os_mem_cpy(drv_priv, &para->setup_info, &accept_i->setup_info,
		sizeof(struct rtw_phl_twt_setup_info));
	psts = phl_cmd_enqueue(phl_info,
				para->sta->rlink->hw_band,
				MSG_EVT_TWT_STA_ACCEPT,
				(u8 *)para,
				para_len,
				_phl_twt_sta_accept_twt_done,
				PHL_CMD_NO_WAIT,
				0);
	if (is_cmd_failure(psts)) {
		/* Send cmd success, but wait cmd fail*/
		psts = RTW_PHL_STATUS_FAILURE;
	} else if (psts != RTW_PHL_STATUS_SUCCESS) {
		/* Send cmd fail */
		_os_kmem_free(drv_priv, para, para_len);
		psts = RTW_PHL_STATUS_FAILURE;
	}
_exit:
	return psts;
}

static void _phl_twt_sta_teardown_done(void *priv, u8 *param,
				u32 param_len, enum rtw_phl_status sts)
{
	if (param) {
		struct rtw_phl_twt_sta_teardown_i *teardown_i =
				(struct rtw_phl_twt_sta_teardown_i *)param;

		if (teardown_i->teardown_done) {
			teardown_i->teardown_done(priv, teardown_i->sta, sts);
		}
		_os_kmem_free(priv, param, param_len);
		param = NULL;
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s\n", __func__);
	}
}

enum rtw_phl_status phl_twt_teardown_for_sta_mode(struct phl_info_t *phl,
				u8 *param)
{
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_sta_teardown_i *teardown_i =
				(struct rtw_phl_twt_sta_teardown_i *)param;
	struct phl_twt_info *phl_twt_info = get_twt_info(phl);
	u8 bitmap =0; /*bitmap of empty config of twt*/
	u8 i = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> %s()\n",
		__func__);
	if (false == twt_sup(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_sup == false\n",
			__func__);
		goto exit;
	}
	if (false == twt_init(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt_init == false\n",
			__func__);
		goto exit;
	}
	pstatus = rtw_phl_twt_teardown_sta(phl, teardown_i->sta,
						&teardown_i->twt_flow, &bitmap);
	if (RTW_PHL_STATUS_SUCCESS != pstatus)
		goto exit;
	if (bitmap == 0)
		goto exit;
	i = 0;
	do {
		if (((bitmap >> i) & BIT0)) {
			pstatus = rtw_phl_twt_free_twt_config(phl, i);
			PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s: sta Q is empty in twt config entry(%d), we free it, pstatus:%d \n",
				__func__, i, pstatus);
		}
		i++;
	} while ((bitmap >> i) != 0);
exit:
	rtw_phl_pkt_ofld_cancel(phl, teardown_i->sta->macid,
			PKT_TYPE_QOS_NULL, &phl_twt_info->qos_null_pkt_token);

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: pstatus(%d)\n",
		__func__, pstatus);
	return pstatus;
}

/*
 * Handle sta to disable twt when sta tx/rx twt teardown frame
 * @struct teardown_i:
 * @phl_sta: sta entry that you wnat to config twt
 * @twt_flow: twt flow field info
 * @teardown_done: callback function after process cmd done
 * Note: for sta mode.
 */
enum rtw_phl_status rtw_phl_twt_teardown_for_sta_mode(void *phl,
			struct rtw_phl_twt_sta_teardown_i *teardown_i)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	void *drv_priv = phl_to_drvpriv(phl_info);
	enum rtw_phl_status psts = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_sta_teardown_i *para = NULL;
	u32 para_len = 0;

	para_len = sizeof(struct rtw_phl_twt_sta_teardown_i);
	para = _os_kmem_alloc(drv_priv, para_len);
	if (para == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s - alloc param failed!\n", __func__);
		goto _exit;
	}
	para->teardown_done = teardown_i->teardown_done;
	para->sta = teardown_i->sta;
	_os_mem_cpy(drv_priv, &para->twt_flow, &teardown_i->twt_flow,
			sizeof(struct rtw_phl_twt_flow_field));
	psts = phl_cmd_enqueue(phl_info,
				para->sta->rlink->hw_band,
				MSG_EVT_TWT_STA_TEARDOWN,
				(u8 *)para,
				para_len,
				_phl_twt_sta_teardown_done,
				PHL_CMD_NO_WAIT,
				0);

	if (is_cmd_failure(psts)) {
		/* Send cmd success, but wait cmd fail*/
		psts = RTW_PHL_STATUS_FAILURE;
	} else if (psts != RTW_PHL_STATUS_SUCCESS) {
		/* Send cmd fail */
		_os_kmem_free(drv_priv, para, para_len);
		psts = RTW_PHL_STATUS_FAILURE;
	}
_exit:
	return psts;
}

static void _phl_twt_info_f_hrl_done(void *priv, u8 *param,
				u32 param_len, enum rtw_phl_status sts)
{
	if (param) {
		_os_kmem_free(priv, param, param_len);
		param = NULL;
		PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "%s\n", __func__);
	}
}

enum rtw_phl_status phl_twt_info_f_hrl(struct phl_info_t *phl, u8 *param)
{
	enum rtw_phl_status sts = RTW_PHL_STATUS_FAILURE;
	struct rtw_phl_twt_info_f_hdr_i *hdr =
					(struct rtw_phl_twt_info_f_hdr_i *)param;
	struct rtw_phl_stainfo_t *sta = hdr->sta;
	struct phl_twt_config *cfg = NULL;
	u8 size_bit = 0;
	u32 tsf_h = 0, tsf_l = 0;
	u64 tsf = 0;

	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "==> %s\n", __func__);
	_dump_twt_info(&hdr->info_f, __FUNCTION__);
	cfg = _twt_get_cfg_by_id(phl, sta, RTW_PHL_INDIV_TWT,
				hdr->info_f.twt_flow_id);
	if (NULL == cfg) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Failed to Get cfg, macid(%d), twt_flow_id(%d)\n",
			__func__, sta->macid, hdr->info_f.twt_flow_id);
		goto _exit;
	}
	if (hdr->info_f.next_twt_size == 0) {
		/* Only suspend */
		_twt_info_update(phl->hal, cfg, PHL_TWT_ACTION_DISABLE);
	} else {
		if (cfg->state == twt_config_state_enable) {
			/* suspend */
			_twt_info_update(phl->hal, cfg, PHL_TWT_ACTION_DISABLE);
		}
		/* resume in the future */
		size_bit = _get_next_twt_subfield_size_bit(hdr->info_f.next_twt_size);
		if (64 == size_bit) {
			tsf = hdr->info_f.next_twt;
			goto _resume;
		}
		if (RTW_HAL_STATUS_SUCCESS != rtw_hal_get_tsf(phl->hal,
							sta->rlink->hw_band,
							sta->rlink->hw_port,
							&tsf_h, &tsf_l)) {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Failed to Get tsf, hw_band(%d), hw_port(%d)\n",
				__func__, sta->rlink->hw_band,
				sta->rlink->hw_port);
			goto _exit;
		}
		tsf = tsf_h;
		tsf = (tsf << 32);
		tsf |= tsf_l;
		if (32 == size_bit) {
			tsf = (tsf & 0xFFFFFFFF00000000) | hdr->info_f.next_twt;
		} else if (48 == size_bit) {
			tsf = (tsf & 0xFFFF000000000000) | hdr->info_f.next_twt;
		} else {
			PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: Error next_twt_size(%d)\n",
				__func__, hdr->info_f.next_twt_size);
			goto _exit;
		}
		_resume:
		cfg->twt_info.target_wake_time_h = (u32)(tsf >> 32);
		cfg->twt_info.target_wake_time_l = (u32)(tsf);
		_twt_info_update(phl->hal, cfg, PHL_TWT_ACTION_ENABLE);
	}
_exit:
	PHL_TRACE(COMP_PHL_TWT, _PHL_INFO_, "<== %s: sts(%d)\n",
		__func__, sts);
	return sts;
}

/*
 * Handle sta to config twt when sta accept the twt agreement
 * @phl_sta: sta entry that you wnat to config twt
 * @setup_info: twt setup info
 * @id: Output the id of twt confi entry
 * Note: for sta mode
 */
enum rtw_phl_status rtw_phl_twt_info_f_hrl(void *phl,
			struct rtw_phl_twt_info_f_hdr_i *hdr)
{
	enum rtw_phl_status psts = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_i = (struct phl_info_t *)phl;
	void *priv = phl_to_drvpriv(phl_i);
	struct rtw_phl_twt_info_f_hdr_i *para = NULL;
	u32 para_len = 0;

	if (false == _twt_valid(phl)) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s: twt invalid\n", __FUNCTION__);
		goto _exit;
	}
	para_len = sizeof(struct rtw_phl_twt_info_f_hdr_i);
	para = _os_kmem_alloc(priv, para_len);
	if (para == NULL) {
		PHL_TRACE(COMP_PHL_TWT, _PHL_ERR_, "%s - alloc param failed!\n",
			__func__);
		goto _exit;
	}
	_os_mem_cpy(priv, para, hdr, sizeof(struct rtw_phl_twt_info_f_hdr_i));
	psts = phl_cmd_enqueue(phl_i,
				para->sta->rlink->hw_band,
				MSG_EVT_TWT_INFO_F_HDR,
				(u8 *)para,
				para_len,
				_phl_twt_info_f_hrl_done,
				PHL_CMD_NO_WAIT,
				0);
	if (is_cmd_failure(psts)) {
		/* Send cmd success, but wait cmd fail*/
		psts = RTW_PHL_STATUS_FAILURE;
	} else if (psts != RTW_PHL_STATUS_SUCCESS) {
		/* Send cmd fail */
		_os_kmem_free(priv, para, para_len);
		psts = RTW_PHL_STATUS_FAILURE;
	}
_exit:
	return psts;
}

#endif /* CONFIG_PHL_TWT */