xref: /OK3568_Linux_fs/external/rkwifibt/drivers/rtl8852be/phl/phl_pkt_ofld.c (revision 4882a59341e53eb6f0b4789bf948001014eff981)

/******************************************************************************
 *
 * Copyright(c) 2019 - 2020 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.
 *
 *****************************************************************************/
#include "phl_headers.h"

static const char *
_phl_pkt_ofld_get_txt(u8 type)
{
	switch (type)
	{
		case PKT_TYPE_PROBE_RSP:
			return "PROBE RSP";
		case PKT_TYPE_PS_POLL:
			return "PS POLL";
		case PKT_TYPE_NULL_DATA:
			return "NULL DATA";
		case PKT_TYPE_QOS_NULL:
			return "QOS NULL";
		case PKT_TYPE_CTS2SELF:
			return "CTS2SELF";
		case PKT_TYPE_ARP_RSP:
			return "ARP Response";
		case PKT_TYPE_NDP:
			return "Neighbor Discovery Protocol";
		case PKT_TYPE_EAPOL_KEY:
			return "EAPOL-KEY";
		case PKT_TYPE_SA_QUERY:
			return "SA QUERY";
		case PKT_TYPE_PROBE_REQ:
			return "PROBE REQ";
		default:
			return "Unknown?!";
	}
}

static u8
_phl_pkt_ofld_is_pkt_ofld(struct pkt_ofld_info *pkt_info)
{
	if(pkt_info->id != NOT_USED)
		return true;
	else
		return false;
}

static void
_phl_pkt_ofld_dbg_dump_pkt_info(struct pkt_ofld_obj *ofld_obj,
				struct pkt_ofld_info *pkt_info)
{
	struct pkt_ofld_req *pos = NULL;

	phl_list_for_loop(pos, struct pkt_ofld_req, &pkt_info->req_q, list) {

		PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
			"[PKT] ## token %d, req name unknown\n",
			pos->token);
	}

	if(_phl_pkt_ofld_is_pkt_ofld(pkt_info)) {
		HAL_PKT_OFLD_READ(ofld_obj, &pkt_info->id);
	}

}

static void
_phl_pkt_ofld_dbg_dump(struct pkt_ofld_obj *ofld_obj)
{
	u8 idx;
	struct pkt_ofld_entry *pos = NULL;

	phl_list_for_loop(pos, struct pkt_ofld_entry, &ofld_obj->entry_q, list) {

		PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
			"[PKT] => mac id = %d\n", pos->macid);

		for(idx = 0; idx < PKT_OFLD_TYPE_MAX; idx++) {

			PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
				"[PKT] type %-10s:id = %d, req cnt = %d.\n",
				_phl_pkt_ofld_get_txt(idx),
				pos->pkt_info[idx].id,
				pos->pkt_info[idx].req_cnt);

			_phl_pkt_ofld_dbg_dump_pkt_info(ofld_obj,
				&pos->pkt_info[idx]);

		}
	}
}

static struct pkt_ofld_req *
_phl_pkt_ofld_gen_req(struct pkt_ofld_obj *ofld_obj, const char *req_name)
{
	struct pkt_ofld_req *req;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	req = _os_mem_alloc(d, sizeof(struct pkt_ofld_req));
	if (req == NULL) {
		PHL_ERR("[PKT] %s: alloc memory req failed.\n", __func__);
		return NULL;
	}

	INIT_LIST_HEAD(&req->list);
	req->token = ofld_obj->cur_seq++;
	req->req_name_len = _os_strlen((u8 *)req_name) +1;

	req->req_name = _os_mem_alloc(d, sizeof(u8)*req->req_name_len);
	if (req->req_name == NULL) {
		PHL_ERR("[PKT] %s: alloc memory req name failed.\n", __func__);
		_os_mem_free(d, req, sizeof(struct pkt_ofld_req));
		return NULL;
	}
	_os_mem_set(d, req->req_name, 0, req->req_name_len);
	_os_mem_cpy(d, req->req_name, (char *)req_name, req->req_name_len);

	PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
		"[PKT] New request %p, token = %d, name = unknown.\n",
		req, req->token);

	return req;
}

static void
_phl_pkt_ofld_del_req(struct pkt_ofld_obj *ofld_obj, struct pkt_ofld_info *pkt_info,
			struct pkt_ofld_req *req)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	list_del(&req->list);
	pkt_info->req_cnt--;
	_os_mem_free(d, req->req_name, sizeof(u8)*req->req_name_len);
	_os_mem_free(d, req, sizeof(*req));
}

static struct pkt_ofld_req *
_phl_pkt_ofld_get_req(struct pkt_ofld_obj *ofld_obj, struct pkt_ofld_info *pkt_info,
			u32 token)
{
	struct pkt_ofld_req *pos = NULL;
	u8 find = false;

	phl_list_for_loop(pos, struct pkt_ofld_req,
				&pkt_info->req_q, list) {
		if (pos->token == token) {
			find = true;
			break;
		}
	}

	if (find) {
		return pos;
	} else {
		return NULL;
	}
}

static void
_phl_pkt_ofld_add_req(struct pkt_ofld_obj *ofld_obj, struct pkt_ofld_info *pkt_info,
			struct pkt_ofld_req *req)
{
	list_add(&req->list, &pkt_info->req_q);
	pkt_info->req_cnt++;
}


static enum rtw_phl_status
_phl_pkt_ofld_construct_null_data(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
	u16 *len, struct rtw_phl_stainfo_t *phl_sta,
	struct rtw_pkt_ofld_null_info *null_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	struct rtw_wifi_role_t *wrole = phl_sta->wrole;
	*len = NULL_PACKET_LEN;

	*pkt_buf = _os_mem_alloc(d, NULL_PACKET_LEN);
	if (*pkt_buf == NULL) {
		return RTW_PHL_STATUS_RESOURCE;
	}

	_os_mem_set(d, *pkt_buf, 0, NULL_PACKET_LEN);

	SET_80211_PKT_HDR_FRAME_CONTROL(*pkt_buf, 0);
	SET_80211_PKT_HDR_TYPE_AND_SUBTYPE(*pkt_buf, TYPE_NULL_FRAME);
	SET_80211_PKT_HDR_PWR_MGNT(*pkt_buf, 0);

	switch(wrole->type)
	{
	case PHL_RTYPE_STATION:
		SET_80211_PKT_HDR_TO_DS(*pkt_buf, 1);
		SET_80211_PKT_HDR_ADDRESS1(d, *pkt_buf, null_info->a1);
		SET_80211_PKT_HDR_ADDRESS2(d, *pkt_buf, null_info->a2);
		SET_80211_PKT_HDR_ADDRESS3(d, *pkt_buf, null_info->a3);
		break;
	case PHL_RTYPE_AP:
	case PHL_RTYPE_ADHOC:
	default:
		break;
	}
	SET_80211_PKT_HDR_DURATION(*pkt_buf, 0);
	SET_80211_PKT_HDR_FRAGMENT_SEQUENCE(*pkt_buf, 0);

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_na(struct pkt_ofld_obj *pkt, u8 **pkt_buf,
	u16 *len, struct rtw_phl_stainfo_t *phl_sta,
	struct rtw_pkt_ofld_na_info *na_info)
{
	void *d = phl_to_drvpriv(pkt->phl_info);
	u8* p_na_body;
	u8 NSLLCHeader[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x86, 0xDD};
	u8 IPv6HeadInfo[4] = {0x60, 0x00, 0x00, 0x00};
	u8 IPv6HeadContx[4] = {0x00, 0x20, 0x3a, 0xff};
	u8 ICMPv6Head[8] = {0x88, 0x00, 0x00, 0x00 , 0x60 , 0x00 , 0x00 , 0x00};
	u8 sec_hdr = na_info->sec_hdr;

	/* size estimation */
	/* sMacHdrLng + LLC header(8) + na element(28) */
	*len = MAC_HDR_LEN + sec_hdr + 8 + 72;

	*pkt_buf = _os_mem_alloc(d, *len);

	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;


	_os_mem_set(d, *pkt_buf, 0, *len);

	SET_80211_PKT_HDR_FRAME_CONTROL(*pkt_buf, 0);
	SET_80211_PKT_HDR_TYPE_AND_SUBTYPE(*pkt_buf, TYPE_DATA_FRAME);
	SET_80211_PKT_HDR_TO_DS(*pkt_buf, 1);

	if (sec_hdr != 0)
		SET_80211_PKT_HDR_PROTECT(*pkt_buf, 1);

	SET_80211_PKT_HDR_ADDRESS1(d, *pkt_buf, na_info->a1);
	SET_80211_PKT_HDR_ADDRESS2(d, *pkt_buf, na_info->a2);
	SET_80211_PKT_HDR_ADDRESS3(d, *pkt_buf, na_info->a3);

	SET_80211_PKT_HDR_DURATION(*pkt_buf, 0);
	SET_80211_PKT_HDR_FRAGMENT_SEQUENCE(*pkt_buf, 0);

	/* Frame Body */
	p_na_body =  (u8*)(*pkt_buf + MAC_HDR_LEN);

	/* offset for security iv */
	p_na_body += sec_hdr;

	/* LLC heade*/
	_os_mem_cpy(d, p_na_body, NSLLCHeader, 8);


	/* IPv6 Heard */


	/* 1 . Information (4 bytes): 0x60 0x00 0x00 0x00 */
	_os_mem_cpy(d, p_na_body+8, IPv6HeadInfo, 4);

	/* 2 . playload : 0x00 0x20 , NextProt : 0x3a (ICMPv6) HopLim : 0xff */
	_os_mem_cpy(d, p_na_body+12, IPv6HeadContx, 4);

	/* 3 . SA : 16 bytes , DA : 16 bytes ( Fw will fill) */
	_os_mem_set(d, p_na_body+16, 0, 32);


	/* ICMPv6 */

	/* 1. Type : 0x88 (NA) , Code : 0x00 , ChechSum : 0x00 0x00 (RSvd) NAFlag: 0x60 0x00 0x00 0x00 ( Solicited , Override) */
	_os_mem_cpy(d, p_na_body+48, ICMPv6Head, 8);

	/* 2. TA : 16 bytes */
	_os_mem_set(d, p_na_body+56, 0, 16);

	/* ICMPv6  Target Link Layer address */

	WriteLE1Byte(p_na_body+72, 0x02); /* type */
	WriteLE1Byte(p_na_body+73, 0x01); /* Len 1 unit of 8 octes */
	_os_mem_set(d, p_na_body+74, 0, 6); /* Target Link Address */

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_arp_rsp(struct pkt_ofld_obj *pkt, u8 **pkt_buf,
	u16 *len, struct rtw_phl_stainfo_t *phl_sta,
	struct rtw_pkt_ofld_arp_rsp_info *arp_rsp_info)
{
	void *d = phl_to_drvpriv(pkt->phl_info);
	u8* p_arp_rsp_body;
	u8 ARPLLCHeader[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x08, 0x06};
	u8 sec_hdr = arp_rsp_info->sec_hdr;


	/* size estimation */
	/* sMacHdrLng + LLC header(8) + arp element(28) */
	*len = MAC_HDR_LEN + sec_hdr + 8 + 28;

	*pkt_buf = _os_mem_alloc(d, *len);

	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;


	_os_mem_set(d, *pkt_buf, 0, *len);

	SET_80211_PKT_HDR_FRAME_CONTROL(*pkt_buf, 0);
	SET_80211_PKT_HDR_TYPE_AND_SUBTYPE(*pkt_buf, TYPE_DATA_FRAME);
	SET_80211_PKT_HDR_TO_DS(*pkt_buf, 1);

	if (sec_hdr != 0)
		SET_80211_PKT_HDR_PROTECT(*pkt_buf, 1);

	SET_80211_PKT_HDR_ADDRESS1(d, *pkt_buf, arp_rsp_info->a1);
	SET_80211_PKT_HDR_ADDRESS2(d, *pkt_buf, arp_rsp_info->a2);
	SET_80211_PKT_HDR_ADDRESS3(d, *pkt_buf, arp_rsp_info->a3);

	SET_80211_PKT_HDR_DURATION(*pkt_buf, 0);
	SET_80211_PKT_HDR_FRAGMENT_SEQUENCE(*pkt_buf, 0);

	/* Frame bod*/
	p_arp_rsp_body = (u8*)(*pkt_buf + MAC_HDR_LEN);

	/* offset for security iv */
	p_arp_rsp_body += sec_hdr;

	/* LLC header */
	_os_mem_cpy(d, p_arp_rsp_body, ARPLLCHeader, 8);

	/* ARP element */
	p_arp_rsp_body += 8;

	/* hardware type  = 0x10 Ethernet */
	WriteLE2Byte(p_arp_rsp_body, 0x0100);
	/* Protcol type = 0800 IP */
	WriteLE2Byte(p_arp_rsp_body+2, 0x0008);
	/* Hardware address length = 6 */
	WriteLE1Byte(p_arp_rsp_body+4, 6);
	/* Protocol address length = 4 */
	WriteLE1Byte(p_arp_rsp_body+5, 4);
	/* Operation = 0x0002 arp response */
	WriteLE2Byte(p_arp_rsp_body+6, 0x0200);
	/* Sender Mac Address */
	_os_mem_cpy(d, p_arp_rsp_body+8, arp_rsp_info->a2, 6);
	/* Sender IPv4 Address */
	_os_mem_cpy(d, p_arp_rsp_body+14,
		&(arp_rsp_info->host_ipv4_addr[0]), 4);
	/* Target Mac Address */
	_os_mem_cpy(d, p_arp_rsp_body+18, arp_rsp_info->a1, 6);
	/* Target IPv4 Address */
	_os_mem_cpy(d, p_arp_rsp_body+24,
		&(arp_rsp_info->remote_ipv4_addr[0]), 4);

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_eapol_key_data(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
				u16 *len, struct rtw_phl_stainfo_t *phl_sta,
				struct rtw_pkt_ofld_eapol_key_info *eapol_key_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	u8 *pkt = NULL;
	u8 llc_hdr[] = {0xAA, 0xAA, 0x03};
	u8 llc_oui[] = {0x00, 0x00, 0x00};
	u8 llc_proto_id[] = {0x88, 0x8E};
	u8 sec_hdr = eapol_key_info->sec_hdr;
	u8 key_desc_ver = eapol_key_info->key_desc_ver;

	*len = MAC_HDR_LEN+sec_hdr+LLC_LEN+AUTH_1X_HDR_LEN+EAPOLMSG_HDR_LEN;

	*pkt_buf = _os_mem_alloc(d, *len);

	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;

	pkt = *pkt_buf;

	_os_mem_set(d, pkt, 0, *len);

	SET_80211_PKT_HDR_FRAME_CONTROL(pkt, 0);
	SET_80211_PKT_HDR_TYPE_AND_SUBTYPE(pkt, TYPE_DATA_FRAME);
	SET_80211_PKT_HDR_TO_DS(pkt, 1);

	if (sec_hdr != 0)
		SET_80211_PKT_HDR_PROTECT(*pkt_buf, 1);

	SET_80211_PKT_HDR_ADDRESS1(d, pkt, eapol_key_info->a1);	/* BSSID */
	SET_80211_PKT_HDR_ADDRESS2(d, pkt, eapol_key_info->a2);	/* SA */
	SET_80211_PKT_HDR_ADDRESS3(d, pkt, eapol_key_info->a3);	/* DA */

	SET_80211_PKT_HDR_DURATION(pkt, 0);
	SET_80211_PKT_HDR_FRAGMENT_SEQUENCE(pkt, 0);
	pkt += MAC_HDR_LEN;

	/* offset for security iv */
	pkt += sec_hdr;

	/* LLC */
	SET_LLC_HDR(d, pkt, llc_hdr);
	SET_LLC_OUI(d, pkt, llc_oui);
	SET_LLC_PROTO_ID(d, pkt, llc_proto_id);
	pkt += LLC_LEN;

	/* 802.1x Auth hdr */
	SET_AUTH_1X_PROTO_VER(pkt, LIB1X_EAPOL_VER);
	SET_AUTH_1X_PKT_TYPE(pkt, LIB1X_TYPE_EAPOL_KEY);
	SET_AUTH_1X_LENGTH(pkt, EAPOLMSG_HDR_LEN);
	pkt += AUTH_1X_HDR_LEN;

	/* EAPOL-KEY */
	SET_EAPOLKEY_KEYDESC_TYPE(pkt, EAPOLKEY_KEYDESC_TYPE_RSN);
	SET_EAPOLKEY_KEY_INFO(pkt, EAPOLKEY_SECURE|EAPOLKEY_KEYMIC|key_desc_ver);

	/* SET_EAPOLKEY_REPLAY_CNT(d, pkt, eapol_key_info->replay_cnt); */

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_sa_query_pkt(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
				u16 *len, struct rtw_phl_stainfo_t *phl_sta,
				struct rtw_pkt_ofld_sa_query_info *sa_query_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	u8 *pkt = NULL;
	u8 sec_hdr = sa_query_info->sec_hdr;

	*len = MAC_HDR_LEN+sec_hdr+SAQ_ACTION_LEN;

	*pkt_buf = _os_mem_alloc(d, *len);

	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;

	pkt = *pkt_buf;

	_os_mem_set(d, pkt, 0, *len);

	SET_80211_PKT_HDR_FRAME_CONTROL(pkt, 0);
	SET_80211_PKT_HDR_TYPE_AND_SUBTYPE(pkt, TYPE_ACTION_FRAME);
	SET_80211_PKT_HDR_DURATION(pkt, 0);

	if (sec_hdr != 0)
		SET_80211_PKT_HDR_PROTECT(pkt, 1);

	SET_80211_PKT_HDR_ADDRESS1(d, pkt, sa_query_info->a1); /* bssid */
	SET_80211_PKT_HDR_ADDRESS2(d, pkt, sa_query_info->a2); /* sa */
	SET_80211_PKT_HDR_ADDRESS3(d, pkt, sa_query_info->a3); /* da */

	SET_80211_PKT_HDR_FRAGMENT_SEQUENCE(pkt, 0);
	pkt += MAC_HDR_LEN;

	/* offset for security iv */
	pkt += sec_hdr;

	SET_ACTION_FRAME_CATEGORY(pkt, ACT_TYPE_SA_QUERY);
	SET_SAQ_ACTION_FIELD(pkt, 1);

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_realwow_kapkt(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
	u16 *len, struct rtw_phl_stainfo_t *phl_sta,
	struct rtw_pkt_ofld_realwow_kapkt_info *kapkt_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	*len = kapkt_info->keep_alive_pkt_size;

	*pkt_buf = _os_mem_alloc(d, *len);
	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;

	_os_mem_set(d, *pkt_buf, 0, *len);

	_os_mem_cpy(d, *pkt_buf, kapkt_info, *len);

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_realwow_ack(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
	u16 *len, struct rtw_phl_stainfo_t *phl_sta,
	struct rtw_pkt_ofld_realwow_ack_info *ack_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	*len = ack_info->ack_ptrn_size;

	*pkt_buf = _os_mem_alloc(d, *len);
	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;

	_os_mem_set(d, *pkt_buf, 0, *len);

	_os_mem_cpy(d, *pkt_buf, ack_info, *len);

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_realwow_wp(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
	u16 *len, struct rtw_phl_stainfo_t *phl_sta,
	struct rtw_pkt_ofld_realwow_wp_info *wake_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	*len = wake_info->wakeup_ptrn_size;

	*pkt_buf = _os_mem_alloc(d, *len);
	if (*pkt_buf == NULL)
		return RTW_PHL_STATUS_RESOURCE;

	_os_mem_set(d, *pkt_buf, 0, *len);

	_os_mem_cpy(d, *pkt_buf, wake_info, *len);

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_probe_req(struct pkt_ofld_obj *ofld_obj, u8 **pkt_buf,
	u16 *len, struct rtw_pkt_ofld_probe_req_info *probe_req_info)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	u8 wild_card[MAC_ADDRESS_LENGTH] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
	u8 *tmp_buf = NULL;

	if (probe_req_info->construct_pbreq == NULL) {

		*len = MAC_HDR_LEN;
		*pkt_buf = _os_mem_alloc(d, MAC_HDR_LEN);
		if (*pkt_buf == NULL) {
			return RTW_PHL_STATUS_RESOURCE;
		}

		_os_mem_set(d, *pkt_buf, 0, MAC_HDR_LEN);

		SET_80211_PKT_HDR_FRAME_CONTROL(*pkt_buf, 0);
		SET_80211_PKT_HDR_TYPE_AND_SUBTYPE(*pkt_buf, TYPE_PROBE_REQ_FRAME);
		SET_80211_PKT_HDR_ADDRESS1(d, *pkt_buf, &wild_card);
		SET_80211_PKT_HDR_ADDRESS2(d, *pkt_buf, probe_req_info->a2);
		SET_80211_PKT_HDR_ADDRESS3(d, *pkt_buf, &wild_card);
		SET_80211_PKT_HDR_DURATION(*pkt_buf, 0);
		SET_80211_PKT_HDR_FRAGMENT_SEQUENCE(*pkt_buf, 0);

	} else {

		tmp_buf = _os_mem_alloc(d, MAX_MSDU_LEN + MAC_HDR_LEN + FCS_LEN);

		if (tmp_buf == NULL)
			return RTW_PHL_STATUS_RESOURCE;

		probe_req_info->construct_pbreq(d, tmp_buf, len);

		*pkt_buf = _os_mem_alloc(d, *len);
		if (*pkt_buf == NULL) {
			_os_mem_free(d, tmp_buf, MAX_MSDU_LEN + MAC_HDR_LEN + FCS_LEN);
			return RTW_PHL_STATUS_RESOURCE;
		}

		_os_mem_set(d, *pkt_buf, 0, *len);
		_os_mem_cpy(d, *pkt_buf, tmp_buf, *len);

		_os_mem_free(d, tmp_buf, MAX_MSDU_LEN + MAC_HDR_LEN + FCS_LEN);
	}

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_construct_packet(struct pkt_ofld_obj *ofld_obj, u16 macid,
			u8 type, u8 **pkt_buf, u16 *len, void *buf)
{
	struct rtw_phl_stainfo_t *phl_sta = rtw_phl_get_stainfo_by_macid(
						ofld_obj->phl_info, macid);
	enum rtw_phl_status status = RTW_PHL_STATUS_SUCCESS;

	switch(type) {
	case PKT_TYPE_NULL_DATA:
		status = _phl_pkt_ofld_construct_null_data(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_null_info *) buf);
		break;
	case PKT_TYPE_ARP_RSP:
		status = _phl_pkt_ofld_construct_arp_rsp(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_arp_rsp_info *) buf);
		break;
	case PKT_TYPE_NDP:
		status = _phl_pkt_ofld_construct_na(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_na_info *) buf);
		break;
	case PKT_TYPE_EAPOL_KEY:
		status = _phl_pkt_ofld_construct_eapol_key_data(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_eapol_key_info *) buf);
		break;
	case PKT_TYPE_SA_QUERY:
		status = _phl_pkt_ofld_construct_sa_query_pkt(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_sa_query_info *) buf);
		break;
	case PKT_TYPE_REALWOW_KAPKT:
		status = _phl_pkt_ofld_construct_realwow_kapkt(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_realwow_kapkt_info *) buf);
		break;
	case PKT_TYPE_REALWOW_ACK:
		status = _phl_pkt_ofld_construct_realwow_ack(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_realwow_ack_info *) buf);
		break;
	case PKT_TYPE_REALWOW_WP:
		status = _phl_pkt_ofld_construct_realwow_wp(ofld_obj, pkt_buf,
			len, phl_sta, (struct rtw_pkt_ofld_realwow_wp_info *) buf);
		break;
	case PKT_TYPE_PROBE_REQ:
		status = _phl_pkt_ofld_construct_probe_req(ofld_obj, pkt_buf,
			len, (struct rtw_pkt_ofld_probe_req_info *) buf);
		break;
	case PKT_TYPE_PROBE_RSP:
	case PKT_TYPE_PS_POLL:
	case PKT_TYPE_QOS_NULL:
	case PKT_TYPE_CTS2SELF:
	default:
		PHL_ERR("[PKT] packet type %s is not implemented.\n",
			_phl_pkt_ofld_get_txt(type));
		status = RTW_PHL_STATUS_FAILURE;
		break;
	}

	if(status == RTW_PHL_STATUS_SUCCESS) {
		PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,"%s:\n", _phl_pkt_ofld_get_txt(type));
		debug_dump_data((u8 *)*pkt_buf, *len, "construct packet");
	} else if(status == RTW_PHL_STATUS_RESOURCE) {
		PHL_ERR("[PKT] %s: alloc memory failed.\n", __func__);
	} else {
		PHL_ERR("[PKT] %s: failed.\n", __func__);
	}

	return status;
}

static void
_phl_pkt_ofld_init_entry(struct pkt_ofld_entry *entry, u16 macid)
{
	u8 idx;

	INIT_LIST_HEAD(&entry->list);
	entry->macid = macid;

	for(idx = 0; idx < PKT_OFLD_TYPE_MAX; idx++) {
		INIT_LIST_HEAD(&entry->pkt_info[idx].req_q);
		entry->pkt_info[idx].id = NOT_USED;
		entry->pkt_info[idx].req_cnt = 0;
	}
}

static u8
_phl_pkt_ofld_is_entry_exist(struct pkt_ofld_obj *ofld_obj, u16 macid)
{
	struct pkt_ofld_entry *pos = NULL;

	phl_list_for_loop(pos, struct pkt_ofld_entry, &ofld_obj->entry_q, list) {
		if (pos->macid == macid) {

			PHL_ERR("[PKT] %s, mac id(%d) already in entry queue.\n",
				__func__, macid);
			return true;
		}
	}

	return false;
}

static void
_phl_pkt_ofld_del_all_req(struct pkt_ofld_obj *ofld_obj,
				struct pkt_ofld_info *pkt_info)
{
	struct pkt_ofld_req *pos = NULL;
	struct pkt_ofld_req *n = NULL;

	phl_list_for_loop_safe(pos, n, struct pkt_ofld_req,
				&pkt_info->req_q, list) {

		_phl_pkt_ofld_del_req(ofld_obj, pkt_info, pos);
	}
}

static void
_phl_pkt_ofld_del_ofld_type(struct pkt_ofld_obj *ofld_obj,
				struct pkt_ofld_entry *entry)
{
	u8 idx;
	u8 id;

	for(idx = 0; idx < PKT_OFLD_TYPE_MAX; idx++) {
		if(_phl_pkt_ofld_is_pkt_ofld(&entry->pkt_info[idx])) {

			id = entry->pkt_info[idx].id;
			if(HAL_PKT_OFLD_DEL(ofld_obj, &id) !=
					RTW_HAL_STATUS_SUCCESS) {

				PHL_ERR("[PKT] %s: delete pkt(%d) failed, id(%d).\n",
					__func__, idx, id);
			}
		}
	}
}

static void
_phl_pkt_ofld_reset_entry(struct pkt_ofld_obj *ofld_obj,
				struct pkt_ofld_entry *entry)
{
	u8 idx;

	for(idx = 0; idx < PKT_OFLD_TYPE_MAX; idx++) {
		entry->pkt_info[idx].id = NOT_USED;
		_phl_pkt_ofld_del_all_req(ofld_obj, &entry->pkt_info[idx]);
	}
}

static void
_phl_pkt_ofld_add_entry(struct pkt_ofld_obj *ofld_obj,
				struct pkt_ofld_entry *entry, u16 macid)
{
	PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
		"[PKT] New entry %p, mac id = %d\n", entry, macid);

	_phl_pkt_ofld_init_entry(entry, macid);
	list_add(&entry->list, &ofld_obj->entry_q);
	ofld_obj->entry_cnt++;

	if(rtw_hal_pkt_update_ids(ofld_obj->phl_info->hal, entry)
					!= RTW_HAL_STATUS_SUCCESS) {
		PHL_WARN("%s: init general id failed.\n", __func__);
	}
}

static void
_phl_pkt_ofld_del_entry(struct pkt_ofld_obj *ofld_obj, struct pkt_ofld_entry *entry)
{
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	list_del(&entry->list);
	ofld_obj->entry_cnt--;
	_os_mem_free(d, entry, sizeof(*entry));
}

static struct pkt_ofld_entry *
_phl_pkt_ofld_get_entry(struct pkt_ofld_obj *ofld_obj, u16 macid)
{
	struct pkt_ofld_entry *pos = NULL;
	u8 find = false;

	phl_list_for_loop(pos, struct pkt_ofld_entry, &ofld_obj->entry_q, list) {
		if (pos->macid == macid) {
			find = true;
			break;
		}
	}

	if (find) {
		return pos;
	} else {
		return NULL;
	}
}

static enum rtw_phl_status
_phl_pkt_ofld_req_type(struct pkt_ofld_obj *ofld_obj,
		struct pkt_ofld_entry *entry, u8 type, void *buf)
{
	enum rtw_phl_status phl_status;
	enum rtw_hal_status hal_status;
	struct pkt_ofld_info *pkt_info = &entry->pkt_info[type];
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	u8 *pkt_buf = NULL;
	u16 len = 0;

	if(_phl_pkt_ofld_is_pkt_ofld(pkt_info)) {
		PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
			"[PKT] %s: type %s is already download to FW.\n",
			__func__, _phl_pkt_ofld_get_txt(type));
		return RTW_PHL_STATUS_SUCCESS;
	}

	phl_status = _phl_pkt_ofld_construct_packet(ofld_obj, entry->macid, type,
			&pkt_buf, &len, buf);
	if(phl_status != RTW_PHL_STATUS_SUCCESS) {
		PHL_ERR("[PKT] %s: construct packet type(%d) failed.\n",
			__func__, type);
		return RTW_PHL_STATUS_FAILURE;
	}

	hal_status = HAL_PKT_OFLD_ADD(ofld_obj, &pkt_info->id, pkt_buf, &len);

	_os_mem_free(d, pkt_buf, len);

	if(hal_status != RTW_HAL_STATUS_SUCCESS) {
		pkt_info->id = NOT_USED;
		PHL_ERR("[PKT] %s: add packet offload(%d) failed.\n", __func__,
				pkt_info->id);
		return RTW_PHL_STATUS_FAILURE;
	}

	hal_status = rtw_hal_pkt_update_ids(ofld_obj->phl_info->hal, entry);
	if(hal_status != RTW_HAL_STATUS_SUCCESS) {
		pkt_info->id = NOT_USED;
		PHL_ERR("[PKT] %s: update id failed.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	return RTW_PHL_STATUS_SUCCESS;
}

static enum rtw_phl_status
_phl_pkt_ofld_cancel_type(struct pkt_ofld_obj *ofld_obj,
				struct pkt_ofld_entry *entry, u8 type)
{
	struct pkt_ofld_info *pkt_info = &entry->pkt_info[type];

	if(_phl_pkt_ofld_is_pkt_ofld(pkt_info) == false) {
		PHL_ERR("[PKT] %s, type not offload to FW.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	if((entry->pkt_info[type].req_cnt-1) != 0) {
		return RTW_PHL_STATUS_SUCCESS;
	}

	if(HAL_PKT_OFLD_DEL(ofld_obj, &entry->pkt_info[type].id)
				!= RTW_HAL_STATUS_SUCCESS) {

		PHL_ERR("[PKT] %s: delete id(%d) failed.\n", __func__,
				entry->pkt_info[type].id);
		return RTW_PHL_STATUS_FAILURE;
	}
	entry->pkt_info[type].id = NOT_USED;

	return RTW_PHL_STATUS_SUCCESS;
}

/* For EXTERNAL application to create packet offload object */
enum rtw_phl_status phl_pkt_ofld_init(struct phl_info_t *phl_info)
{
	void *d = phl_to_drvpriv(phl_info);
	struct pkt_ofld_obj *ofld_obj;

	ofld_obj = (struct pkt_ofld_obj *)_os_mem_alloc(d, sizeof(*ofld_obj));
	if (ofld_obj == NULL)
		return RTW_PHL_STATUS_RESOURCE;

	phl_info->pkt_ofld = ofld_obj;

	ofld_obj->phl_info = phl_info;
	INIT_LIST_HEAD(&ofld_obj->entry_q);
	ofld_obj->entry_cnt = 0;
	ofld_obj->cur_seq = 0;
	_os_mutex_init(d, &ofld_obj->mux);

	return RTW_PHL_STATUS_SUCCESS;
}

/* For EXTERNAL application to free packet offload object */
void phl_pkt_ofld_deinit(struct phl_info_t *phl_info)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	struct pkt_ofld_entry *pos = NULL;
	struct pkt_ofld_entry *n = NULL;
	void *d = phl_to_drvpriv(phl_info);

	_os_mutex_lock(d, &ofld_obj->mux);

	phl_list_for_loop_safe(pos, n, struct pkt_ofld_entry,
				&ofld_obj->entry_q, list) {

		_phl_pkt_ofld_reset_entry(ofld_obj, pos);
		_phl_pkt_ofld_del_entry(ofld_obj, pos);
	}

	_os_mutex_unlock(d, &ofld_obj->mux);
	_os_mutex_deinit(d, &ofld_obj->mux);

	_os_mem_free(d, ofld_obj, sizeof(*ofld_obj));
}

/* For EXTERNAL application to reset all entry */
void phl_pkt_ofld_reset_all_entry(struct phl_info_t *phl_info)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	struct pkt_ofld_entry *pos = NULL;
	void *d = phl_to_drvpriv(phl_info);

	_os_mutex_lock(d, &ofld_obj->mux);

	phl_list_for_loop(pos, struct pkt_ofld_entry,
				&ofld_obj->entry_q, list) {

		_phl_pkt_ofld_reset_entry(ofld_obj, pos);
	}

	_os_mutex_unlock(d, &ofld_obj->mux);
}

/* For EXTERNAL application to add entry to do packet offload (expose)
 * @phl: refer to rtw_phl_com_t
 * @macid: the mac id of STA
 */
enum rtw_phl_status phl_pkt_ofld_add_entry(struct phl_info_t *phl_info, u16 macid)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	struct pkt_ofld_entry *entry = NULL;

	if (ofld_obj == NULL) {
		PHL_ERR("[PKT] %s: pkt_ofld_obj is NULL.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	_os_mutex_lock(d, &ofld_obj->mux);

	if (_phl_pkt_ofld_is_entry_exist(ofld_obj, macid)) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		return RTW_PHL_STATUS_FAILURE;
	}

	entry = _os_mem_alloc(d, sizeof(*entry));
	if (entry == NULL) {
		PHL_ERR("[PKT] %s: alloc memory failed.\n", __func__);
		_os_mutex_unlock(d, &ofld_obj->mux);
		return RTW_PHL_STATUS_RESOURCE;
	}

	_phl_pkt_ofld_add_entry(ofld_obj, entry, macid);

	_os_mutex_unlock(d, &ofld_obj->mux);

	return RTW_PHL_STATUS_SUCCESS;
}

/* For EXTERNAL application to remove entry (expose)
 * @phl: refer to rtw_phl_com_t
 * @macid: the mac id of STA
*/
enum rtw_phl_status phl_pkt_ofld_del_entry(struct phl_info_t *phl_info, u16 macid)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	struct pkt_ofld_entry *entry = NULL;

	if (ofld_obj == NULL) {
		PHL_ERR("[PKT] %s: pkt_ofld_obj is NULL.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	_os_mutex_lock(d, &ofld_obj->mux);

	entry = _phl_pkt_ofld_get_entry(ofld_obj, macid);
	if(entry == NULL) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		PHL_ERR("[PKT] %s, mac id(%d) not found.\n", __func__, macid);
		return RTW_PHL_STATUS_FAILURE;
	}

	PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
		"[PKT] Remove entry %p, mac id = %d\n", entry, macid);

	_phl_pkt_ofld_del_ofld_type(ofld_obj, entry);
	_phl_pkt_ofld_reset_entry(ofld_obj, entry);
	_phl_pkt_ofld_del_entry(ofld_obj, entry);

	_os_mutex_unlock(d, &ofld_obj->mux);

	return RTW_PHL_STATUS_SUCCESS;
}

/* For EXTERNAL application to request packet offload to FW (expose)
 * @phl: refer to rtw_phl_com_t
 * @macid: the mac id of STA
 * @type: The type of packet
 * @token: The identifier (return to caller)
 * @req_name: The function name of caller
*/
enum rtw_phl_status
phl_pkt_ofld_request(struct phl_info_t *phl_info, u16 macid, u8 type,
		u32 *token, const char *req_name, void *buf)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	struct pkt_ofld_entry *entry = NULL;
	struct pkt_ofld_req *req = NULL;

	if (ofld_obj == NULL) {
		PHL_ERR("[PKT] %s: pkt_ofld_obj is NULL.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	_os_mutex_lock(d, &ofld_obj->mux);

	entry = _phl_pkt_ofld_get_entry(ofld_obj, macid);
	if(entry == NULL) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		PHL_ERR("[PKT] %s, mac id(%d) not found.\n", __func__, macid);
		return RTW_PHL_STATUS_FAILURE;
	}

	req = _phl_pkt_ofld_gen_req(ofld_obj, req_name);
	if (req == NULL) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		return RTW_PHL_STATUS_RESOURCE;
	}
	_phl_pkt_ofld_add_req(ofld_obj, &entry->pkt_info[type], req);

	if(_phl_pkt_ofld_req_type(ofld_obj, entry, type, buf) !=
			RTW_PHL_STATUS_SUCCESS) {

		_phl_pkt_ofld_del_req(ofld_obj, &entry->pkt_info[type], req);
		_os_mutex_unlock(d, &ofld_obj->mux);
		return RTW_PHL_STATUS_FAILURE;
	}

	*token = req->token;

	_os_mutex_unlock(d, &ofld_obj->mux);

	PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
		"[PKT] Request: macid %d, pkt type %s, token %d.\n",
		entry->macid, _phl_pkt_ofld_get_txt(type), *token);

	return RTW_PHL_STATUS_SUCCESS;
}

/* For EXTERNAL application to cancel request (expose)
 * @phl: refer to rtw_phl_com_t
 * @macid: the mac id of STA
 * @type: The type of packet
 * @token: The identifier to get the request to be canceled
*/
enum rtw_phl_status phl_pkt_ofld_cancel(struct phl_info_t *phl_info,
					u16 macid, u8 type, u32 *token)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	struct pkt_ofld_entry *entry = NULL;
	struct pkt_ofld_req *req = NULL;
	struct pkt_ofld_info *pkt_info = NULL;

	if (ofld_obj == NULL) {
		PHL_ERR("[PKT] %s: pkt_ofld_obj is NULL.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	_os_mutex_lock(d, &ofld_obj->mux);

	entry = _phl_pkt_ofld_get_entry(ofld_obj, macid);
	if(entry == NULL) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		PHL_ERR("[PKT] %s, macid(%d) not found.\n", __func__, macid);
		return RTW_PHL_STATUS_FAILURE;
	}
	pkt_info = &entry->pkt_info[type];

	req = _phl_pkt_ofld_get_req(ofld_obj, pkt_info, *token);
	if(req == NULL) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		PHL_ERR("[PKT] %s, token(%d) not found.\n", __func__, *token);
		return RTW_PHL_STATUS_FAILURE;
	}

	PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
		"[PKT] Cancel: macid %d, type %s, token %d.\n",
		entry->macid, _phl_pkt_ofld_get_txt(type), *token);

	if(_phl_pkt_ofld_cancel_type(ofld_obj, entry, type)
			!= RTW_PHL_STATUS_SUCCESS) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		return RTW_PHL_STATUS_FAILURE;
	}
	_phl_pkt_ofld_del_req(ofld_obj, pkt_info, req);

	_os_mutex_unlock(d, &ofld_obj->mux);

	return RTW_PHL_STATUS_SUCCESS;
}

/* For EXTERNAL application to show current info (expose)
 * @phl: refer to rtw_phl_com_t
*/
void phl_pkt_ofld_show_info(struct phl_info_t *phl_info)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);

	_os_mutex_lock(d, &ofld_obj->mux);

	_phl_pkt_ofld_dbg_dump(ofld_obj);

	_os_mutex_unlock(d, &ofld_obj->mux);
}

/* For EXTERNAL application to get id (expose)
 * @phl: refer to rtw_phl_com_t
 * @macid: the mac id of STA
 * @type: The type of packet
*/
u8 phl_pkt_ofld_get_id(struct phl_info_t *phl_info, u16 macid, u8 type)
{
	struct pkt_ofld_obj *ofld_obj = phl_info->pkt_ofld;
	void *d = phl_to_drvpriv(ofld_obj->phl_info);
	struct pkt_ofld_entry *entry = NULL;
	struct pkt_ofld_info *pkt_info = NULL;

	if (ofld_obj == NULL) {
		PHL_ERR("[PKT] %s: pkt_ofld_obj is NULL.\n", __func__);
		return RTW_PHL_STATUS_FAILURE;
	}

	_os_mutex_lock(d, &ofld_obj->mux);

	entry = _phl_pkt_ofld_get_entry(ofld_obj, macid);
	if(entry == NULL) {
		_os_mutex_unlock(d, &ofld_obj->mux);
		PHL_ERR("[PKT] %s, macid(%d) not found.\n", __func__, macid);
		return RTW_PHL_STATUS_FAILURE;
	}
	pkt_info = &entry->pkt_info[type];

	PHL_TRACE(COMP_PHL_PKTOFLD, _PHL_INFO_,
		"[PKT] Get id: macid %d, pkt type %s, id %d.\n",
		entry->macid, _phl_pkt_ofld_get_txt(type),
		pkt_info->id);

	_os_mutex_unlock(d, &ofld_obj->mux);

	return pkt_info->id;
}

const char *phl_get_pkt_ofld_str(enum pkt_ofld_type type)
{
	switch(type) {
	case PKT_TYPE_NULL_DATA:
		return "PKT_TYPE_NULL_DATA";
	case PKT_TYPE_ARP_RSP:
		return "PKT_TYPE_ARP_RSP";
	case PKT_TYPE_NDP:
		return "PKT_TYPE_NDP";
	case PKT_TYPE_EAPOL_KEY:
		return "PKT_TYPE_EAPOL_KEY";
	case PKT_TYPE_SA_QUERY:
		return "PKT_TYPE_SA_QUERY";
	case PKT_TYPE_PROBE_REQ:
		return "PKT_TYPE_PROBE_REQ";
	default:
		return "UNKNOWN_PKT_TYPE";
	}
}