diff options
Diffstat (limited to 'drivers/staging/rtl8723au/core/rtw_p2p.c')
| -rw-r--r-- | drivers/staging/rtl8723au/core/rtw_p2p.c | 4001 |
1 files changed, 4001 insertions, 0 deletions
diff --git a/drivers/staging/rtl8723au/core/rtw_p2p.c b/drivers/staging/rtl8723au/core/rtw_p2p.c new file mode 100644 index 000000000000..27a6cc76973d --- /dev/null +++ b/drivers/staging/rtl8723au/core/rtw_p2p.c @@ -0,0 +1,4001 @@ +/****************************************************************************** + * + * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved. + * + * 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 _RTW_P2P_C_ + +#include <drv_types.h> +#include <rtw_p2p.h> +#include <wifi.h> + +#ifdef CONFIG_8723AU_P2P + +static int rtw_p2p_is_channel_list_ok(u8 desired_ch, u8* ch_list, u8 ch_cnt) +{ + int found = 0, i = 0; + + for (i = 0; i < ch_cnt; i++) + { + if (ch_list[ i ] == desired_ch) + { + found = 1; + break; + } + } + return found; +} + +static int is_any_client_associated(struct rtw_adapter *padapter) +{ + return padapter->stapriv.asoc_list_cnt ? true : false; +} + +static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + struct list_head *phead, *plist; + u32 len = 0; + u16 attr_len = 0; + u8 tmplen, *pdata_attr, *pstart, *pcur; + struct sta_info *psta; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct sta_priv *pstapriv = &padapter->stapriv; + + DBG_8723A("%s\n", __func__); + + pdata_attr = kzalloc(MAX_P2P_IE_LEN, GFP_ATOMIC); + + pstart = pdata_attr; + pcur = pdata_attr; + + spin_lock_bh(&pstapriv->asoc_list_lock); + phead = &pstapriv->asoc_list; + + list_for_each(plist, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + if (psta->is_p2p_device) + { + tmplen = 0; + + pcur++; + + /* P2P device address */ + memcpy(pcur, psta->dev_addr, ETH_ALEN); + pcur += ETH_ALEN; + + /* P2P interface address */ + memcpy(pcur, psta->hwaddr, ETH_ALEN); + pcur += ETH_ALEN; + + *pcur = psta->dev_cap; + pcur++; + + /* u16*)(pcur) = cpu_to_be16(psta->config_methods); */ + put_unaligned_be16(psta->config_methods, pcur); + pcur += 2; + + memcpy(pcur, psta->primary_dev_type, 8); + pcur += 8; + + *pcur = psta->num_of_secdev_type; + pcur++; + + memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8); + pcur += psta->num_of_secdev_type*8; + + if (psta->dev_name_len>0) + { + /* u16*)(pcur) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */ + put_unaligned_be16(WPS_ATTR_DEVICE_NAME, pcur); + pcur += 2; + + /* u16*)(pcur) = cpu_to_be16(psta->dev_name_len); */ + put_unaligned_be16(psta->dev_name_len, pcur); + pcur += 2; + + memcpy(pcur, psta->dev_name, psta->dev_name_len); + pcur += psta->dev_name_len; + } + + tmplen = (u8)(pcur-pstart); + + *pstart = (tmplen-1); + + attr_len += tmplen; + + /* pstart += tmplen; */ + pstart = pcur; + + } + + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + + if (attr_len>0) + { + len = rtw_set_p2p_attr_content23a(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr); + } + + kfree(pdata_attr); + + return len; +} + +static void issue_group_disc_req(struct wifidirect_info *pwdinfo, u8 *da) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_GO_DISC_REQUEST; + u8 dialogToken = 0; + + DBG_8723A("[%s]\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN); + memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* Build P2P action frame header */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *)&p2poui, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + /* there is no IE in this P2P action frame */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +static void issue_p2p_devdisc_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_DEVDISC_RESP; + u8 p2pie[8] = { 0x00 }; + u32 p2pielen = 0; + + DBG_8723A("[%s]\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, pwdinfo->device_addr, ETH_ALEN); + memcpy(pwlanhdr->addr3, pwdinfo->device_addr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* Build P2P public action frame header */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + /* Build P2P IE */ + /* P2P OUI */ + p2pielen = 0; + p2pie[ p2pielen++ ] = 0x50; + p2pie[ p2pielen++ ] = 0x6F; + p2pie[ p2pielen++ ] = 0x9A; + p2pie[ p2pielen++ ] = 0x09; /* WFA P2P v1.0 */ + + /* P2P_ATTR_STATUS */ + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status); + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, &pattrib->pktlen); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method) +{ + struct rtw_adapter *padapter = pwdinfo->padapter; + unsigned char category = WLAN_CATEGORY_PUBLIC; + u8 action = P2P_PUB_ACTION_ACTION; + u8 dialogToken = frame_body[7]; /* The Dialog Token of provisioning discovery request frame. */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PROVISION_DISC_RESP; + u8 wpsie[ 100 ] = { 0x00 }; + u8 wpsielen = 0; +#ifdef CONFIG_8723AU_P2P + u32 wfdielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + return; + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + memcpy(pwlanhdr->addr1, raddr, ETH_ALEN); + memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN); + memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &action, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + wpsielen = 0; + /* WPS OUI */ + /* u32*) (wpsie) = cpu_to_be32(WPSOUI); */ + put_unaligned_be32(WPSOUI, wpsie); + wpsielen += 4; + + /* Config Method */ + /* Type: */ + /* u16*) (wpsie + wpsielen) = cpu_to_be16(WPS_ATTR_CONF_METHOD); */ + put_unaligned_be16(WPS_ATTR_CONF_METHOD, wpsie + wpsielen); + wpsielen += 2; + + /* Length: */ + /* u16*) (wpsie + wpsielen) = cpu_to_be16(0x0002); */ + put_unaligned_be16(0x0002, wpsie + wpsielen); + wpsielen += 2; + + /* Value: */ + /* u16*) (wpsie + wpsielen) = cpu_to_be16(config_method); */ + put_unaligned_be16(config_method, wpsie + wpsielen); + wpsielen += 2; + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen); + +#ifdef CONFIG_8723AU_P2P + wfdielen = build_provdisc_resp_wfd_ie(pwdinfo, pframe); + pframe += wfdielen; + pattrib->pktlen += wfdielen; +#endif /* CONFIG_8723AU_P2P */ + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); + + return; +} + +static void issue_p2p_presence_resp(struct wifidirect_info *pwdinfo, u8 *da, u8 status, u8 dialogToken) +{ + struct xmit_frame *pmgntframe; + struct pkt_attrib *pattrib; + unsigned char *pframe; + struct ieee80211_hdr *pwlanhdr; + unsigned short *fctrl; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct xmit_priv *pxmitpriv = &padapter->xmitpriv; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + unsigned char category = WLAN_CATEGORY_VENDOR_SPECIFIC;/* P2P action frame */ + u32 p2poui = cpu_to_be32(P2POUI); + u8 oui_subtype = P2P_PRESENCE_RESPONSE; + u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 }; + u8 noa_attr_content[32] = { 0x00 }; + u32 p2pielen = 0; + + DBG_8723A("[%s]\n", __func__); + + if ((pmgntframe = alloc_mgtxmitframe23a(pxmitpriv)) == NULL) + { + return; + } + + /* update attribute */ + pattrib = &pmgntframe->attrib; + update_mgntframe_attrib23a(padapter, pattrib); + + memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET); + + pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET; + pwlanhdr = (struct ieee80211_hdr *)pframe; + + fctrl = &pwlanhdr->frame_control; + *fctrl = 0; + + memcpy(pwlanhdr->addr1, da, ETH_ALEN); + memcpy(pwlanhdr->addr2, pwdinfo->interface_addr, ETH_ALEN); + memcpy(pwlanhdr->addr3, pwdinfo->interface_addr, ETH_ALEN); + + SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq); + pmlmeext->mgnt_seq++; + SetFrameSubType(pframe, WIFI_ACTION); + + pframe += sizeof(struct ieee80211_hdr_3addr); + pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr); + + /* Build P2P action frame header */ + pframe = rtw_set_fixed_ie23a(pframe, 1, &category, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 4, (unsigned char *) &p2poui, + &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &oui_subtype, &pattrib->pktlen); + pframe = rtw_set_fixed_ie23a(pframe, 1, &dialogToken, &pattrib->pktlen); + + /* Add P2P IE header */ + /* P2P OUI */ + p2pielen = 0; + p2pie[ p2pielen++ ] = 0x50; + p2pie[ p2pielen++ ] = 0x6F; + p2pie[ p2pielen++ ] = 0x9A; + p2pie[ p2pielen++ ] = 0x09; /* WFA P2P v1.0 */ + + /* Add Status attribute in P2P IE */ + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status); + + /* Add NoA attribute in P2P IE */ + noa_attr_content[0] = 0x1;/* index */ + noa_attr_content[1] = 0x0;/* CTWindow and OppPS Parameters */ + + /* todo: Notice of Absence Descriptor(s) */ + + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_NOA, 2, noa_attr_content); + + pframe = rtw_set_ie23a(pframe, _VENDOR_SPECIFIC_IE_, p2pielen, p2pie, + &pattrib->pktlen); + + pattrib->last_txcmdsz = pattrib->pktlen; + + dump_mgntframe23a(padapter, pmgntframe); +} + +u32 build_beacon_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 }; + u16 capability = 0; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[ p2pielen++ ] = 0x50; + p2pie[ p2pielen++ ] = 0x6F; + p2pie[ p2pielen++ ] = 0x9A; + p2pie[ p2pielen++ ] = 0x09; /* WFA P2P v1.0 */ + + /* According to the P2P Specification, the beacon frame should contain 3 P2P attributes */ + /* 1. P2P Capability */ + /* 2. P2P Device ID */ + /* 3. Notice of Absence (NOA) */ + + /* P2P Capability ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + /* Be able to participate in additional P2P Groups and */ + /* support the P2P Invitation Procedure */ + /* Group Capability Bitmap, 1 byte */ + capability = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY; + capability |= ((P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS) << 8); + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING)) + capability |= (P2P_GRPCAP_GROUP_FORMATION<<8); + + capability = cpu_to_le16(capability); + + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_CAPABILITY, 2, (u8*)&capability); + + /* P2P Device ID ATTR */ + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_DEVICE_ID, ETH_ALEN, pwdinfo->device_addr); + + /* Notice of Absence ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + + /* go_add_noa_attr(pwdinfo); */ + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +#ifdef CONFIG_8723AU_P2P +u32 build_beacon_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the beacon frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + + if (P2P_ROLE_GO == pwdinfo->role) + { + if (is_any_client_associated(pwdinfo->padapter)) + { + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_WSD, wfdie + wfdielen); + } + else + { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD, wfdie + wfdielen); + } + + } + else + { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD, wfdie + wfdielen); + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_probe_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + + if (1 == pwdinfo->wfd_tdls_enable) + { + /* WFD primary sink + available for WFD session + WiFi TDLS mode + WSC (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD | + WFD_DEVINFO_PC_TDLS, wfdie + wfdielen); + } + else + { + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSC (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD, wfdie + wfdielen); + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_probe_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 tunneled) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe response frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + /* 4. WFD Session Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode */ + + if (true == pwdinfo->session_available) + { + if (P2P_ROLE_GO == pwdinfo->role) + { + if (is_any_client_associated(pwdinfo->padapter)) + { + if (pwdinfo->wfd_tdls_enable) + { + /* TDLS mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen); + } + else + { + /* WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen); + } + } + else + { + if (pwdinfo->wfd_tdls_enable) + { + /* available for WFD session + TDLS mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_PC_TDLS | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen); + } + else + { + /* available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD | WFD_DEVINFO_HDCP_SUPPORT, wfdie + wfdielen); + } + } + } + else + { + if (pwdinfo->wfd_tdls_enable) + { + /* available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD | + WFD_DEVINFO_PC_TDLS | + WFD_DEVINFO_HDCP_SUPPORT, + wfdie + wfdielen); + } + else + { + + /* available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD | + WFD_DEVINFO_HDCP_SUPPORT, + wfdie + wfdielen); + } + } + } + else + { + if (pwdinfo->wfd_tdls_enable) + { + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_WSD | + WFD_DEVINFO_PC_TDLS | + WFD_DEVINFO_HDCP_SUPPORT, + wfdie + wfdielen); + } + else + { + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_WSD | + WFD_DEVINFO_HDCP_SUPPORT, + wfdie + wfdielen); + } + + } + + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0000, wfdie + wfdielen); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_assoc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = NULL; + struct mlme_priv *pmlmepriv = NULL; + struct wifi_display_info *pwfd_info = NULL; + + /* WFD OUI */ + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_IDLE)) + { + return 0; + } + + padapter = pwdinfo->padapter; + pmlmepriv = &padapter->mlmepriv; + pwfd_info = padapter->wdinfo.wfd_info; + + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD, wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_assoc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110812 */ + /* According to the WFD Specification, the probe request frame should contain 4 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID */ + /* 3. Coupled Sink Information */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | + WFD_DEVINFO_WSD, wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_nego_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL, + wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_nego_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_WSD | WFD_DEVINFO_SESSION_AVAIL, + wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_nego_confirm_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the negotiation request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + WiFi Direct mode + WSD (WFD Service Discovery) + WFD Session Available */ + put_unaligned_be16(pwfd_info->wfd_device_type | WFD_DEVINFO_WSD | + WFD_DEVINFO_SESSION_AVAIL, wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_invitation_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD, + wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + if (P2P_ROLE_GO == pwdinfo->role) + { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0000, wfdie + wfdielen); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_invitation_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD, + wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + if (P2P_ROLE_GO == pwdinfo->role) + { + /* WFD Session Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_SESSION_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0000, wfdie + wfdielen); + wfdielen += 2; + + /* Todo: to add the list of WFD device info descriptor in WFD group. */ + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_provdisc_req_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery request frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD, + wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +u32 build_provdisc_resp_wfd_ie(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 wfdie[ MAX_WFD_IE_LEN] = { 0x00 }; + u32 len = 0, wfdielen = 0; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct wifi_display_info* pwfd_info = padapter->wdinfo.wfd_info; + + /* WFD OUI */ + wfdielen = 0; + wfdie[ wfdielen++ ] = 0x50; + wfdie[ wfdielen++ ] = 0x6F; + wfdie[ wfdielen++ ] = 0x9A; + wfdie[ wfdielen++ ] = 0x0A; /* WFA WFD v1.0 */ + + /* Commented by Albert 20110825 */ + /* According to the WFD Specification, the provision discovery response frame should contain 3 WFD attributes */ + /* 1. WFD Device Information */ + /* 2. Associated BSSID (Optional) */ + /* 3. Local IP Adress (Optional) */ + + /* WFD Device Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_DEVICE_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value1: */ + /* WFD device information */ + /* WFD primary sink + available for WFD session + WiFi Direct mode + WSD (WFD Service Discovery) */ + put_unaligned_be16(pwfd_info->wfd_device_type | + WFD_DEVINFO_SESSION_AVAIL | WFD_DEVINFO_WSD, + wfdie + wfdielen); + wfdielen += 2; + + /* Value2: */ + /* Session Management Control Port */ + /* Default TCP port for RTSP messages is 554 */ + put_unaligned_be16(pwfd_info->rtsp_ctrlport, wfdie + wfdielen); + wfdielen += 2; + + /* Value3: */ + /* WFD Device Maximum Throughput */ + /* 300Mbps is the maximum throughput */ + put_unaligned_be16(300, wfdie + wfdielen); + wfdielen += 2; + + /* Associated BSSID ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_ASSOC_BSSID; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0006, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Associated BSSID */ + if (check_fwstate(pmlmepriv, _FW_LINKED) == true) + { + memcpy(wfdie + wfdielen, &pmlmepriv->assoc_bssid[ 0 ], ETH_ALEN); + } + else + { + memset(wfdie + wfdielen, 0x00, ETH_ALEN); + } + + wfdielen += ETH_ALEN; + + /* Coupled Sink Information ATTR */ + /* Type: */ + wfdie[ wfdielen++ ] = WFD_ATTR_COUPLED_SINK_INFO; + + /* Length: */ + /* Note: In the WFD specification, the size of length field is 2. */ + put_unaligned_be16(0x0007, wfdie + wfdielen); + wfdielen += 2; + + /* Value: */ + /* Coupled Sink Status bitmap */ + /* Not coupled/available for Coupling */ + wfdie[ wfdielen++ ] = 0; + /* MAC Addr. */ + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + wfdie[ wfdielen++ ] = 0; + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, wfdielen, (unsigned char *) wfdie, &len); + + return len; +} + +#endif /* CONFIG_8723AU_P2P */ + +u32 build_probe_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 }; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[ p2pielen++ ] = 0x50; + p2pie[ p2pielen++ ] = 0x6F; + p2pie[ p2pielen++ ] = 0x9A; + p2pie[ p2pielen++ ] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20100907 */ + /* According to the P2P Specification, the probe response frame should contain 5 P2P attributes */ + /* 1. P2P Capability */ + /* 2. Extended Listen Timing */ + /* 3. Notice of Absence (NOA) (Only GO needs this) */ + /* 4. Device Info */ + /* 5. Group Info (Only GO need this) */ + + /* P2P Capability ATTR */ + /* Type: */ + p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY; + + /* Length: */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */ + put_unaligned_le16(0x0002, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_PROVISIONING_ING)) + p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION; + + p2pielen++; + } + else if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) + { + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT; + else + p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT; + } + + /* Extended Listen Timing ATTR */ + /* Type: */ + p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING; + + /* Length: */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0004); */ + put_unaligned_le16(0x0004, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + /* Availability Period */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */ + put_unaligned_le16(0xFFFF, p2pie + p2pielen); + p2pielen += 2; + + /* Availability Interval */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(0xFFFF); */ + put_unaligned_le16(0xFFFF, p2pie + p2pielen); + p2pielen += 2; + + /* Notice of Absence ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + /* go_add_noa_attr(pwdinfo); */ + } + + /* Device Info ATTR */ + /* Type: */ + p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */ + put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->supported_wps_cm); */ + put_unaligned_be16(pwdinfo->supported_wps_cm, p2pie + p2pielen); + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */ + put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen); + p2pielen += 2; + + /* OUI */ + /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */ + put_unaligned_be32(WPSOUI, p2pie + p2pielen); + p2pielen += 4; + + /* Sub Category ID */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */ + put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[ p2pielen++ ] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */ + put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen); + p2pielen += 2; + + /* Length: */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */ + put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + /* Group Info ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen); + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +u32 build_prov_disc_request_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr) +{ + u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 }; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[ p2pielen++ ] = 0x50; + p2pie[ p2pielen++ ] = 0x6F; + p2pie[ p2pielen++ ] = 0x9A; + p2pie[ p2pielen++ ] = 0x09; /* WFA P2P v1.0 */ + + /* Commented by Albert 20110301 */ + /* According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes */ + /* 1. P2P Capability */ + /* 2. Device Info */ + /* 3. Group ID (When joining an operating P2P Group) */ + + /* P2P Capability ATTR */ + /* Type: */ + p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY; + + /* Length: */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(0x0002); */ + put_unaligned_le16(0x0002, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + /* Device Capability Bitmap, 1 byte */ + p2pie[ p2pielen++ ] = DMP_P2P_DEVCAP_SUPPORT; + + /* Group Capability Bitmap, 1 byte */ + if (pwdinfo->persistent_supported) + p2pie[ p2pielen++ ] = P2P_GRPCAP_PERSISTENT_GROUP | DMP_P2P_GRPCAP_SUPPORT; + else + p2pie[ p2pielen++ ] = DMP_P2P_GRPCAP_SUPPORT; + + /* Device Info ATTR */ + /* Type: */ + p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO; + + /* Length: */ + /* 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes) */ + /* + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes) */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(21 + pwdinfo->device_name_len); */ + put_unaligned_le16(21 + pwdinfo->device_name_len, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + /* P2P Device Address */ + memcpy(p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN); + p2pielen += ETH_ALEN; + + /* Config Method */ + /* This field should be big endian. Noted by P2P specification. */ + if (pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC) + { + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_PBC); */ + put_unaligned_be16(WPS_CONFIG_METHOD_PBC, p2pie + p2pielen); + } + else + { + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_CONFIG_METHOD_DISPLAY); */ + put_unaligned_be16(WPS_CONFIG_METHOD_DISPLAY, p2pie + p2pielen); + } + + p2pielen += 2; + + /* Primary Device Type */ + /* Category ID */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_CID_MULIT_MEDIA); */ + put_unaligned_be16(WPS_PDT_CID_MULIT_MEDIA, p2pie + p2pielen); + p2pielen += 2; + + /* OUI */ + /* u32*) (p2pie + p2pielen) = cpu_to_be32(WPSOUI); */ + put_unaligned_be32(WPSOUI, p2pie + p2pielen); + p2pielen += 4; + + /* Sub Category ID */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_PDT_SCID_MEDIA_SERVER); */ + put_unaligned_be16(WPS_PDT_SCID_MEDIA_SERVER, p2pie + p2pielen); + p2pielen += 2; + + /* Number of Secondary Device Types */ + p2pie[ p2pielen++ ] = 0x00; /* No Secondary Device Type List */ + + /* Device Name */ + /* Type: */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(WPS_ATTR_DEVICE_NAME); */ + put_unaligned_be16(WPS_ATTR_DEVICE_NAME, p2pie + p2pielen); + p2pielen += 2; + + /* Length: */ + /* u16*) (p2pie + p2pielen) = cpu_to_be16(pwdinfo->device_name_len); */ + put_unaligned_be16(pwdinfo->device_name_len, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + memcpy(p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len); + p2pielen += pwdinfo->device_name_len; + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_CLIENT)) + { + /* Added by Albert 2011/05/19 */ + /* In this case, the pdev_raddr is the device address of the group owner. */ + + /* P2P Group ID ATTR */ + /* Type: */ + p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID; + + /* Length: */ + /* u16*) (p2pie + p2pielen) = cpu_to_le16(ETH_ALEN + ussidlen); */ + put_unaligned_le16(ETH_ALEN + ussidlen, p2pie + p2pielen); + p2pielen += 2; + + /* Value: */ + memcpy(p2pie + p2pielen, pdev_raddr, ETH_ALEN); + p2pielen += ETH_ALEN; + + memcpy(p2pie + p2pielen, pssid, ussidlen); + p2pielen += ussidlen; + + } + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +u32 build_assoc_resp_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf, u8 status_code) +{ + u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 }; + u32 len = 0, p2pielen = 0; + + /* P2P OUI */ + p2pielen = 0; + p2pie[ p2pielen++ ] = 0x50; + p2pie[ p2pielen++ ] = 0x6F; + p2pie[ p2pielen++ ] = 0x9A; + p2pie[ p2pielen++ ] = 0x09; /* WFA P2P v1.0 */ + + /* According to the P2P Specification, the Association response frame should contain 2 P2P attributes */ + /* 1. Status */ + /* 2. Extended Listen Timing (optional) */ + + /* Status ATTR */ + p2pielen += rtw_set_p2p_attr_content23a(&p2pie[p2pielen], P2P_ATTR_STATUS, 1, &status_code); + + /* Extended Listen Timing ATTR */ + /* Type: */ + /* Length: */ + /* Value: */ + + pbuf = rtw_set_ie23a(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len); + + return len; +} + +u32 build_deauth_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pbuf) +{ + u32 len = 0; + + return len; +} + +u32 process_probe_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *p; + u32 ret = false; + u8 *p2pie; + u32 p2pielen = 0; + int ssid_len = 0, rate_cnt = 0; + + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt, + len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_); + + if (rate_cnt <= 4) + { + int i, g_rate = 0; + + for (i = 0; i < rate_cnt; i++) + { + if (((*(p + 2 + i) & 0xff) != 0x02) && + ((*(p + 2 + i) & 0xff) != 0x04) && + ((*(p + 2 + i) & 0xff) != 0x0B) && + ((*(p + 2 + i) & 0xff) != 0x16)) + { + g_rate = 1; + } + } + + if (g_rate == 0) + { + /* There is no OFDM rate included in SupportedRates IE of this probe request frame */ + /* The driver should response this probe request. */ + return ret; + } + } + else + { + /* rate_cnt > 4 means the SupportRates IE contains the OFDM rate because the count of CCK rates are 4. */ + /* We should proceed the following check for this probe request. */ + } + + /* Added comments by Albert 20100906 */ + /* There are several items we should check here. */ + /* 1. This probe request frame must contain the P2P IE. (Done) */ + /* 2. This probe request frame must contain the wildcard SSID. (Done) */ + /* 3. Wildcard BSSID. (Todo) */ + /* 4. Destination Address. (Done in mgt_dispatcher23a function) */ + /* 5. Requested Device Type in WSC IE. (Todo) */ + /* 6. Device ID attribute in P2P IE. (Todo) */ + + p = rtw_get_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len, + len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_); + + ssid_len &= 0xff; /* Just last 1 byte is valid for ssid len of the probe request */ + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE) || rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + if ((p2pie = rtw_get_p2p_ie23a(pframe + sizeof(struct ieee80211_hdr_3addr) + _PROBEREQ_IE_OFFSET_, len - sizeof(struct ieee80211_hdr_3addr) - _PROBEREQ_IE_OFFSET_, NULL, &p2pielen))) + { + if ((p) && !memcmp((void *)(p+2), (void *)pwdinfo->p2p_wildcard_ssid, 7)) + { + /* todo: */ + /* Check Requested Device Type attributes in WSC IE. */ + /* Check Device ID attribute in P2P IE */ + + ret = true; + } + else if ((p != NULL) && (ssid_len == 0)) + { + ret = true; + } + } + else + { + /* non -p2p device */ + } + + } + + return ret; +} + +u32 process_assoc_req_p2p_ie23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len, struct sta_info *psta) +{ + u8 status_code = P2P_STATUS_SUCCESS; + u8 *pbuf, *pattr_content = NULL; + u32 attr_contentlen = 0; + u16 cap_attr = 0; + unsigned short ie_offset; + u8 * ies; + u32 ies_len; + u8 * p2p_ie; + u32 p2p_ielen = 0; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe; + + if (!rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + return P2P_STATUS_FAIL_REQUEST_UNABLE; + + if (ieee80211_is_assoc_req(hdr->frame_control)) + ie_offset = _ASOCREQ_IE_OFFSET_; + else /* WIFI_REASSOCREQ */ + ie_offset = _REASOCREQ_IE_OFFSET_; + + ies = pframe + sizeof(struct ieee80211_hdr_3addr) + ie_offset; + ies_len = len - sizeof(struct ieee80211_hdr_3addr) - ie_offset; + + p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen); + + if (!p2p_ie) + { + DBG_8723A("[%s] P2P IE not Found!!\n", __func__); + status_code = P2P_STATUS_FAIL_INVALID_PARAM; + } + else + { + DBG_8723A("[%s] P2P IE Found!!\n", __func__); + } + + while (p2p_ie) + { + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*) &attr_contentlen)) + { + DBG_8723A("[%s] Got P2P Capability Attr!!\n", __func__); + cap_attr = le16_to_cpu(cap_attr); + psta->dev_cap = cap_attr&0xff; + } + + /* Check Extended Listen Timing ATTR */ + + /* Check P2P Device Info ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen)) + { + DBG_8723A("[%s] Got P2P DEVICE INFO Attr!!\n", __func__); + pattr_content = pbuf = kzalloc(attr_contentlen, + GFP_ATOMIC); + if (pattr_content) { + u8 num_of_secdev_type; + u16 dev_name_len; + + rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, pattr_content, (uint*)&attr_contentlen); + + memcpy(psta->dev_addr, pattr_content, ETH_ALEN);/* P2P Device Address */ + + pattr_content += ETH_ALEN; + + memcpy(&psta->config_methods, pattr_content, 2);/* Config Methods */ + psta->config_methods = be16_to_cpu(psta->config_methods); + + pattr_content += 2; + + memcpy(psta->primary_dev_type, pattr_content, 8); + + pattr_content += 8; + + num_of_secdev_type = *pattr_content; + pattr_content += 1; + + if (num_of_secdev_type == 0) + { + psta->num_of_secdev_type = 0; + } + else + { + u32 len; + + psta->num_of_secdev_type = num_of_secdev_type; + + len = (sizeof(psta->secdev_types_list)<(num_of_secdev_type*8)) ? (sizeof(psta->secdev_types_list)) : (num_of_secdev_type*8); + + memcpy(psta->secdev_types_list, pattr_content, len); + + pattr_content += (num_of_secdev_type*8); + } + + /* dev_name_len = attr_contentlen - ETH_ALEN - 2 - 8 - 1 - (num_of_secdev_type*8); */ + psta->dev_name_len = 0; + if (WPS_ATTR_DEVICE_NAME == be16_to_cpu(*(u16*)pattr_content)) + { + dev_name_len = be16_to_cpu(*(u16*)(pattr_content+2)); + + psta->dev_name_len = (sizeof(psta->dev_name)<dev_name_len) ? sizeof(psta->dev_name):dev_name_len; + + memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len); + } + + kfree(pbuf); + + } + + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + + } + + return status_code; +} + +u32 process_p2p_devdisc_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, + uint len) +{ + u8 *frame_body; + u8 status, dialogToken; + struct sta_info *psta = NULL; + struct rtw_adapter *padapter = pwdinfo->padapter; + struct sta_priv *pstapriv = &padapter->stapriv; + u8 *p2p_ie; + u32 p2p_ielen = 0; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe; + + frame_body = (unsigned char *) + (pframe + sizeof(struct ieee80211_hdr_3addr)); + + dialogToken = frame_body[7]; + status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP; + + if ((p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + len - _PUBLIC_ACTION_IE_OFFSET_, NULL, + &p2p_ielen))) { + u8 groupid[38] = { 0x00 }; + u8 dev_addr[ETH_ALEN] = { 0x00 }; + u32 attr_contentlen = 0; + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, + P2P_ATTR_GROUP_ID, groupid, + &attr_contentlen)) { + if (!memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) && + !memcmp(pwdinfo->p2p_group_ssid, groupid + ETH_ALEN, + pwdinfo->p2p_group_ssid_len)) { + attr_contentlen = 0; + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, + P2P_ATTR_DEVICE_ID, + dev_addr, + &attr_contentlen)) { + struct list_head *phead, *plist, *ptmp; + + spin_lock_bh(&pstapriv->asoc_list_lock); + phead = &pstapriv->asoc_list; + + list_for_each_safe(plist, ptmp, phead) { + psta = container_of(plist, struct sta_info, asoc_list); + + if (psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) && + !memcmp(psta->dev_addr, dev_addr, ETH_ALEN)) + { + /* spin_unlock_bh(&pstapriv->asoc_list_lock); */ + /* issue GO Discoverability Request */ + issue_group_disc_req(pwdinfo, psta->hwaddr); + /* spin_lock_bh(&pstapriv->asoc_list_lock); */ + status = P2P_STATUS_SUCCESS; + break; + } else { + status = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + } + } + spin_unlock_bh(&pstapriv->asoc_list_lock); + } else { + status = P2P_STATUS_FAIL_INVALID_PARAM; + } + } else { + status = P2P_STATUS_FAIL_INVALID_PARAM; + } + } + } + + /* issue Device Discoverability Response */ + issue_p2p_devdisc_resp(pwdinfo, hdr->addr2, status, dialogToken); + + return (status == P2P_STATUS_SUCCESS) ? true:false; +} + +u32 process_p2p_devdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + return true; +} + +u8 process_p2p_provdisc_req23a(struct wifidirect_info *pwdinfo, + u8 *pframe, uint len) +{ + u8 *frame_body; + u8 *wpsie; + u8 *ptr = NULL; + uint wps_ielen = 0, attr_contentlen = 0; + u16 uconfig_method = 0; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)pframe; + + frame_body = (pframe + sizeof(struct ieee80211_hdr_3addr)); + + wpsie = rtw_get_wps_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + len - _PUBLIC_ACTION_IE_OFFSET_, NULL, + &wps_ielen); + if (!wpsie) + goto out; + + if (!rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_CONF_METHOD, + (u8 *)&uconfig_method, &attr_contentlen)) + goto out; + + uconfig_method = be16_to_cpu(uconfig_method); + ptr = pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req; + + switch (uconfig_method) + { + case WPS_CM_DISPLYA: + memcpy(ptr, "dis", 3); + break; + + case WPS_CM_LABEL: + memcpy(ptr, "lab", 3); + break; + + case WPS_CM_PUSH_BUTTON: + memcpy(ptr, "pbc", 3); + break; + + case WPS_CM_KEYPAD: + memcpy(ptr, "pad", 3); + break; + } + issue_p2p_provision_resp(pwdinfo, hdr->addr2, frame_body, + uconfig_method); + +out: + DBG_8723A("[%s] config method = %s\n", __func__, ptr); + + return true; +} + +u8 process_p2p_provdisc_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe) +{ + + return true; +} + +static u8 rtw_p2p_get_peer_ch_list(struct wifidirect_info *pwdinfo, u8 *ch_content, u8 ch_cnt, u8 *peer_ch_list) +{ + u8 i = 0, j = 0; + u8 temp = 0; + u8 ch_no = 0; + ch_content += 3; + ch_cnt -= 3; + + while(ch_cnt > 0) + { + ch_content += 1; + ch_cnt -= 1; + temp = *ch_content; + for (i = 0 ; i < temp ; i++, j++) + { + peer_ch_list[j] = *(ch_content + 1 + i); + } + ch_content += (temp + 1); + ch_cnt -= (temp + 1); + ch_no += temp ; + } + + return ch_no; +} + +static u8 rtw_p2p_ch_inclusion(struct mlme_ext_priv *pmlmeext, u8 *peer_ch_list, u8 peer_ch_num, u8 *ch_list_inclusioned) +{ + int i = 0, j = 0, temp = 0; + u8 ch_no = 0; + + for (i = 0; i < peer_ch_num; i++) + { + for (j = temp; j < pmlmeext->max_chan_nums; j++) + { + if (*(peer_ch_list + i) == pmlmeext->channel_set[ j ].ChannelNum) + { + ch_list_inclusioned[ ch_no++ ] = *(peer_ch_list + i); + temp = j; + break; + } + } + } + + return ch_no; +} + +u8 process_p2p_group_negotation_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + struct rtw_adapter *padapter = pwdinfo->padapter; + u8 result = P2P_STATUS_SUCCESS; + u32 p2p_ielen = 0, wps_ielen = 0; + u8 * ies; + u32 ies_len; + u8 *p2p_ie; + u8 *wpsie; + u16 wps_devicepassword_id = 0x0000; + uint wps_devicepassword_id_len = 0; +#ifdef CONFIG_8723AU_P2P + u8 wfd_ie[ 128 ] = { 0x00 }; + u32 wfd_ielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + if ((wpsie = rtw_get_wps_ie23a(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, NULL, &wps_ielen))) + { + /* Commented by Kurt 20120113 */ + /* If some device wants to do p2p handshake without sending prov_disc_req */ + /* We have to get peer_req_cm from here. */ + if (!memcmp(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "000", 3)) + { + rtw_get_wps_attr_content23a(wpsie, wps_ielen, WPS_ATTR_DEVICE_PWID, (u8*) &wps_devicepassword_id, &wps_devicepassword_id_len); + wps_devicepassword_id = be16_to_cpu(wps_devicepassword_id); + + if (wps_devicepassword_id == WPS_DPID_USER_SPEC) + { + memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "dis", 3); + } + else if (wps_devicepassword_id == WPS_DPID_REGISTRAR_SPEC) + { + memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pad", 3); + } + else + { + memcpy(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, "pbc", 3); + } + } + } + else + { + DBG_8723A("[%s] WPS IE not Found!!\n", __func__); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + return result; + } + + if (pwdinfo->ui_got_wps_info == P2P_NO_WPSINFO) + { + result = P2P_STATUS_FAIL_INFO_UNAVAILABLE; + rtw_p2p_set_state(pwdinfo, P2P_STATE_TX_INFOR_NOREADY); + return result; + } + + ies = pframe + _PUBLIC_ACTION_IE_OFFSET_; + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen); + + if (!p2p_ie) + { + DBG_8723A("[%s] P2P IE not Found!!\n", __func__); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } + + while (p2p_ie) + { + u8 attr_content = 0x00; + u32 attr_contentlen = 0; + u8 ch_content[50] = { 0x00 }; + uint ch_cnt = 0; + u8 peer_ch_list[50] = { 0x00 }; + u8 peer_ch_num = 0; + u8 ch_list_inclusioned[50] = { 0x00 }; + u8 ch_num_inclusioned = 0; + u16 cap_attr; + + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_ING); + + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen)) + cap_attr = le16_to_cpu(cap_attr); + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen)) + { + DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01); + pwdinfo->peer_intent = attr_content; /* include both intent and tie breaker values. */ + + if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) + { + /* Try to match the tie breaker value */ + if (pwdinfo->intent == P2P_MAX_INTENT) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + result = P2P_STATUS_FAIL_BOTH_GOINTENT_15; + } + else + { + if (attr_content & 0x01) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + else + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + } + } + else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + else + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + /* Store the group id information. */ + memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN); + memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen); + } + } + + attr_contentlen = 0; + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) + { + if (attr_contentlen != ETH_ALEN) + { + memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN); + } + } + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, ch_content, &ch_cnt)) + { + peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, ch_content, ch_cnt, peer_ch_list); + ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned); + + if (ch_num_inclusioned == 0) + { + DBG_8723A("[%s] No common channel in channel list!\n", __func__); + result = P2P_STATUS_FAIL_NO_COMMON_CH; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel, + ch_list_inclusioned, ch_num_inclusioned)) + { + { + u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0; + attr_contentlen = 0; + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + peer_operating_ch = operatingch_info[4]; + } + + if (rtw_p2p_is_channel_list_ok(peer_operating_ch, + ch_list_inclusioned, ch_num_inclusioned)) + { + /** + * Change our operating channel as peer's for compatibility. + */ + pwdinfo->operating_channel = peer_operating_ch; + DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel); + } + else + { + /* Take first channel of ch_list_inclusioned as operating channel */ + pwdinfo->operating_channel = ch_list_inclusioned[0]; + DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel); + } + } + + } + } + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + } + +#ifdef CONFIG_8723AU_P2P + /* Added by Albert 20110823 */ + /* Try to get the TCP port information when receiving the negotiation request. */ + if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen)) + { + u8 attr_content[ 10 ] = { 0x00 }; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] WFD IE Found!!\n", __func__); + rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen); + if (attr_contentlen) + { + pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2); + DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport); + } + } +#endif /* CONFIG_8723AU_P2P */ + + return result; +} + +u8 process_p2p_group_negotation_resp23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + struct rtw_adapter *padapter = pwdinfo->padapter; + u8 result = P2P_STATUS_SUCCESS; + u32 p2p_ielen, wps_ielen; + u8 * ies; + u32 ies_len; + u8 * p2p_ie; +#ifdef CONFIG_8723AU_P2P + u8 wfd_ie[ 128 ] = { 0x00 }; + u32 wfd_ielen = 0; +#endif /* CONFIG_8723AU_P2P */ + + ies = pframe + _PUBLIC_ACTION_IE_OFFSET_; + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + /* Be able to know which one is the P2P GO and which one is P2P client. */ + + if (rtw_get_wps_ie23a(ies, ies_len, NULL, &wps_ielen)) + { + + } + else + { + DBG_8723A("[%s] WPS IE not Found!!\n", __func__); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } + + p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen); + if (!p2p_ie) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + result = P2P_STATUS_FAIL_INCOMPATIBLE_PARAM; + } + else + { + + u8 attr_content = 0x00; + u32 attr_contentlen = 0; + u8 operatingch_info[5] = { 0x00 }; + u8 groupid[ 38 ]; + u16 cap_attr; + u8 peer_ch_list[50] = { 0x00 }; + u8 peer_ch_num = 0; + u8 ch_list_inclusioned[50] = { 0x00 }; + u8 ch_num_inclusioned = 0; + + while (p2p_ie) /* Found the P2P IE. */ + { + + /* Check P2P Capability ATTR */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen)) + cap_attr = le16_to_cpu(cap_attr); + + rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + if (attr_contentlen == 1) + { + DBG_8723A("[%s] Status = %d\n", __func__, attr_content); + if (attr_content == P2P_STATUS_SUCCESS) + { + /* Do nothing. */ + } + else + { + if (P2P_STATUS_FAIL_INFO_UNAVAILABLE == attr_content) { + rtw_p2p_set_state(pwdinfo, P2P_STATE_RX_INFOR_NOREADY); + } else { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + result = attr_content; + break; + } + } + + /* Try to get the peer's interface address */ + attr_contentlen = 0; + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INTENTED_IF_ADDR, pwdinfo->p2p_peer_interface_addr, &attr_contentlen)) + { + if (attr_contentlen != ETH_ALEN) + { + memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN); + } + } + + /* Try to get the peer's intent and tie breaker value. */ + attr_content = 0x00; + attr_contentlen = 0; + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GO_INTENT, &attr_content, &attr_contentlen)) + { + DBG_8723A("[%s] GO Intent = %d, tie = %d\n", __func__, attr_content >> 1, attr_content & 0x01); + pwdinfo->peer_intent = attr_content; /* include both intent and tie breaker values. */ + + if (pwdinfo->intent == (pwdinfo->peer_intent >> 1)) + { + /* Try to match the tie breaker value */ + if (pwdinfo->intent == P2P_MAX_INTENT) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + result = P2P_STATUS_FAIL_BOTH_GOINTENT_15; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + } + else + { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + if (attr_content & 0x01) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + else + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + } + } + else if (pwdinfo->intent > (pwdinfo->peer_intent >> 1)) + { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + else + { + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + /* Store the group id information. */ + memcpy(pwdinfo->groupid_info.go_device_addr, pwdinfo->device_addr, ETH_ALEN); + memcpy(pwdinfo->groupid_info.ssid, pwdinfo->nego_ssid, pwdinfo->nego_ssidlen); + + } + } + + /* Try to get the operation channel information */ + + attr_contentlen = 0; + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]); + pwdinfo->peer_operating_ch = operatingch_info[4]; + } + + /* Try to get the channel list information */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_CH_LIST, pwdinfo->channel_list_attr, &pwdinfo->channel_list_attr_len)) + { + DBG_8723A("[%s] channel list attribute found, len = %d\n", __func__, pwdinfo->channel_list_attr_len); + + peer_ch_num = rtw_p2p_get_peer_ch_list(pwdinfo, pwdinfo->channel_list_attr, pwdinfo->channel_list_attr_len, peer_ch_list); + ch_num_inclusioned = rtw_p2p_ch_inclusion(&padapter->mlmeextpriv, peer_ch_list, peer_ch_num, ch_list_inclusioned); + + if (ch_num_inclusioned == 0) + { + DBG_8723A("[%s] No common channel in channel list!\n", __func__); + result = P2P_STATUS_FAIL_NO_COMMON_CH; + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_GO)) + { + if (!rtw_p2p_is_channel_list_ok(pwdinfo->operating_channel, + ch_list_inclusioned, ch_num_inclusioned)) + { + { + u8 operatingch_info[5] = { 0x00 }, peer_operating_ch = 0; + attr_contentlen = 0; + + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + peer_operating_ch = operatingch_info[4]; + } + + if (rtw_p2p_is_channel_list_ok(peer_operating_ch, + ch_list_inclusioned, ch_num_inclusioned)) + { + /** + * Change our operating channel as peer's for compatibility. + */ + pwdinfo->operating_channel = peer_operating_ch; + DBG_8723A("[%s] Change op ch to %02x as peer's\n", __func__, pwdinfo->operating_channel); + } + else + { + /* Take first channel of ch_list_inclusioned as operating channel */ + pwdinfo->operating_channel = ch_list_inclusioned[0]; + DBG_8723A("[%s] Change op ch to %02x\n", __func__, pwdinfo->operating_channel); + } + } + + } + } + + } + else + { + DBG_8723A("[%s] channel list attribute not found!\n", __func__); + } + + /* Try to get the group id information if peer is GO */ + attr_contentlen = 0; + memset(groupid, 0x00, 38); + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) + { + memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN); + memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN); + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + } + + } + +#ifdef CONFIG_8723AU_P2P + /* Added by Albert 20111122 */ + /* Try to get the TCP port information when receiving the negotiation response. */ + if (rtw_get_wfd_ie(pframe + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, wfd_ie, &wfd_ielen)) + { + u8 attr_content[ 10 ] = { 0x00 }; + u32 attr_contentlen = 0; + + DBG_8723A("[%s] WFD IE Found!!\n", __func__); + rtw_get_wfd_attr_content(wfd_ie, wfd_ielen, WFD_ATTR_DEVICE_INFO, attr_content, &attr_contentlen); + if (attr_contentlen) + { + pwdinfo->wfd_info->peer_rtsp_ctrlport = get_unaligned_be16(attr_content + 2); + DBG_8723A("[%s] Peer PORT NUM = %d\n", __func__, pwdinfo->wfd_info->peer_rtsp_ctrlport); + } + } +#endif /* CONFIG_8723AU_P2P */ + + return result; +} + +u8 process_p2p_group_negotation_confirm23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 * ies; + u32 ies_len; + u8 * p2p_ie; + u32 p2p_ielen = 0; + u8 result = P2P_STATUS_SUCCESS; + ies = pframe + _PUBLIC_ACTION_IE_OFFSET_; + ies_len = len - _PUBLIC_ACTION_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen); + while (p2p_ie) /* Found the P2P IE. */ + { + u8 attr_content = 0x00, operatingch_info[5] = { 0x00 }; + u8 groupid[ 38 ] = { 0x00 }; + u32 attr_contentlen = 0; + + pwdinfo->negotiation_dialog_token = 1; + rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, &attr_content, &attr_contentlen); + if (attr_contentlen == 1) + { + DBG_8723A("[%s] Status = %d\n", __func__, attr_content); + result = attr_content; + + if (attr_content == P2P_STATUS_SUCCESS) + { + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + + /* Commented by Albert 20100911 */ + /* Todo: Need to handle the case which both Intents are the same. */ + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + if ((pwdinfo->intent) > (pwdinfo->peer_intent >> 1)) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + else if ((pwdinfo->intent) < (pwdinfo->peer_intent >> 1)) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + else + { + /* Have to compare the Tie Breaker */ + if (pwdinfo->peer_intent & 0x01) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + } + else + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + } + } + } + else + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_FAIL); + break; + } + } + + /* Try to get the group id information */ + attr_contentlen = 0; + memset(groupid, 0x00, 38); + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen)) + { + DBG_8723A("[%s] Ssid = %s, ssidlen = %zu\n", __func__, &groupid[ETH_ALEN], strlen(&groupid[ETH_ALEN])); + memcpy(pwdinfo->groupid_info.go_device_addr, &groupid[0], ETH_ALEN); + memcpy(pwdinfo->groupid_info.ssid, &groupid[6], attr_contentlen - ETH_ALEN); + } + + attr_contentlen = 0; + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, operatingch_info, &attr_contentlen)) + { + DBG_8723A("[%s] Peer's operating channel = %d\n", __func__, operatingch_info[4]); + pwdinfo->peer_operating_ch = operatingch_info[4]; + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + + } + + return result; +} + +u8 process_p2p_presence_req23a(struct wifidirect_info *pwdinfo, u8 *pframe, uint len) +{ + u8 *frame_body; + u8 dialogToken = 0; + u8 status = P2P_STATUS_SUCCESS; + struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) pframe; + + frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr)); + + dialogToken = frame_body[6]; + + /* todo: check NoA attribute */ + + issue_p2p_presence_resp(pwdinfo, hdr->addr2, status, dialogToken); + + return true; +} + +static void find_phase_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + struct mlme_priv *pmlmepriv = &padapter->mlmepriv; + struct cfg80211_ssid ssid; + u8 _status = 0; + + + + memset((unsigned char*)&ssid, 0, sizeof(struct cfg80211_ssid)); + memcpy(ssid.ssid, pwdinfo->p2p_wildcard_ssid, P2P_WILDCARD_SSID_LEN); + ssid.ssid_len = P2P_WILDCARD_SSID_LEN; + + rtw_p2p_set_state(pwdinfo, P2P_STATE_FIND_PHASE_SEARCH); + + spin_lock_bh(&pmlmepriv->lock); + _status = rtw_sitesurvey_cmd23a(padapter, &ssid, 1, NULL, 0); + spin_unlock_bh(&pmlmepriv->lock); + + +} + +void p2p_concurrent_handler(struct rtw_adapter* padapter); + +static void restore_p2p_state_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING) || rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_FAIL)) + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + + if (rtw_p2p_chk_role(pwdinfo, P2P_ROLE_DEVICE)) { + /* In the P2P client mode, the driver should not switch back to its listen channel */ + /* because this P2P client should stay at the operating channel of P2P GO. */ + set_channel_bwmode23a(padapter, pwdinfo->listen_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + } +} + +static void pre_tx_invitereq_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + set_channel_bwmode23a(padapter, pwdinfo->invitereq_info.peer_ch, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue23a_probereq_p2p(padapter, NULL); + mod_timer(&pwdinfo->pre_tx_scan_timer, + jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT)); + + +} + +static void pre_tx_provdisc_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + + set_channel_bwmode23a(padapter, pwdinfo->tx_prov_disc_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue23a_probereq_p2p(padapter, NULL); + mod_timer(&pwdinfo->pre_tx_scan_timer, + jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT)); + + +} + +static void pre_tx_negoreq_handler(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 val8 = 1; + + + set_channel_bwmode23a(padapter, pwdinfo->nego_req_info.peer_channel_num[0], HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20); + rtw_hal_set_hwreg23a(padapter, HW_VAR_MLME_SITESURVEY, (u8 *)(&val8)); + issue23a_probereq_p2p(padapter, NULL); + mod_timer(&pwdinfo->pre_tx_scan_timer, + jiffies + msecs_to_jiffies(P2P_TX_PRESCAN_TIMEOUT)); + + +} + +static void ro_ch_handler(struct rtw_adapter *padapter) +{ + struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv; + + if (pcfg80211_wdinfo->restore_channel != pmlmeext->cur_channel) { + if (!check_fwstate(&padapter->mlmepriv, _FW_LINKED)) + pmlmeext->cur_channel = pcfg80211_wdinfo->restore_channel; + + set_channel_bwmode23a(padapter, pmlmeext->cur_channel, + HAL_PRIME_CHNL_OFFSET_DONT_CARE, + HT_CHANNEL_WIDTH_20); + } + + rtw_p2p_set_state(pwdinfo, rtw_p2p_pre_state(pwdinfo)); + + pcfg80211_wdinfo->is_ro_ch = false; + + DBG_8723A("cfg80211_remain_on_channel_expired\n"); + + rtw_cfg80211_remain_on_channel_expired(padapter, + pcfg80211_wdinfo->remain_on_ch_cookie, + &pcfg80211_wdinfo->remain_on_ch_channel, + pcfg80211_wdinfo->remain_on_ch_type, GFP_KERNEL); +} + +static void ro_ch_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + + p2p_protocol_wk_cmd23a(adapter, P2P_RO_CH_WK); +} + +#ifdef CONFIG_8723AU_P2P +void rtw_append_wfd_ie(struct rtw_adapter *padapter, u8 *buf, u32* len) +{ + unsigned char *frame_body; + u8 category, action, OUI_Subtype, dialogToken = 0; + u32 wfdielen = 0; + + frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr)); + category = frame_body[0]; + + if (category == WLAN_CATEGORY_PUBLIC) { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR && + !memcmp(frame_body+2, P2P_OUI23A, 4)) { + OUI_Subtype = frame_body[6]; + dialogToken = frame_body[7]; + switch (OUI_Subtype)/* OUI Subtype */ { + case P2P_GO_NEGO_REQ: + wfdielen = build_nego_req_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_GO_NEGO_RESP: + wfdielen = build_nego_resp_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_GO_NEGO_CONF: + wfdielen = build_nego_confirm_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_INVIT_REQ: + wfdielen = build_invitation_req_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_INVIT_RESP: + wfdielen = build_invitation_resp_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_DEVDISC_REQ: + break; + case P2P_DEVDISC_RESP: + break; + case P2P_PROVISION_DISC_REQ: + wfdielen = build_provdisc_req_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + case P2P_PROVISION_DISC_RESP: + wfdielen = build_provdisc_resp_wfd_ie(&padapter->wdinfo, buf + (*len)); + (*len) += wfdielen; + break; + default: + break; + } + } + } else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) { + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + } else { + DBG_8723A("%s, action frame category =%d\n", __func__, category); + } +} +#endif + +int rtw_p2p_check_frames(struct rtw_adapter *padapter, const u8 *buf, u32 len, u8 tx) +{ + int is_p2p_frame = (-1); + unsigned char *frame_body; + u8 category, action, OUI_Subtype, dialogToken = 0; + u8 *p2p_ie = NULL; + uint p2p_ielen = 0; + struct rtw_wdev_priv *pwdev_priv = wdev_to_priv(padapter->rtw_wdev); + + frame_body = (unsigned char *)(buf + sizeof(struct ieee80211_hdr_3addr)); + category = frame_body[0]; + /* just for check */ + if (category == WLAN_CATEGORY_PUBLIC) + { + action = frame_body[1]; + if (action == ACT_PUBLIC_VENDOR && + !memcmp(frame_body+2, P2P_OUI23A, 4)) { + OUI_Subtype = frame_body[6]; + dialogToken = frame_body[7]; + is_p2p_frame = OUI_Subtype; + p2p_ie = rtw_get_p2p_ie23a( + (u8 *)buf+sizeof(struct ieee80211_hdr_3addr)+_PUBLIC_ACTION_IE_OFFSET_, + len-sizeof(struct ieee80211_hdr_3addr)-_PUBLIC_ACTION_IE_OFFSET_, + NULL, &p2p_ielen); + + switch (OUI_Subtype) {/* OUI Subtype */ + u8 *cont; + uint cont_len; + case P2P_GO_NEGO_REQ: + DBG_8723A("RTW_%s:P2P_GO_NEGO_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + break; + case P2P_GO_NEGO_RESP: + cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + DBG_8723A("RTW_%s:P2P_GO_NEGO_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1); + + if (!tx) + pwdev_priv->provdisc_req_issued = false; + break; + case P2P_GO_NEGO_CONF: + cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + DBG_8723A("RTW_%s:P2P_GO_NEGO_CONF, dialogToken =%d, status:%d\n", + (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1); + break; + case P2P_INVIT_REQ: + { + struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info; + int flags = -1; + int op_ch = 0; + + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_INVITATION_FLAGS, NULL, &cont_len))) + flags = *cont; + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len))) + op_ch = *(cont+4); + + if (invit_info->token != dialogToken) + rtw_wdev_invit_info_init(invit_info); + + invit_info->token = dialogToken; + invit_info->flags = (flags ==-1) ? 0x0 : flags; + invit_info->req_op_ch = op_ch; + + DBG_8723A("RTW_%s:P2P_INVIT_REQ, dialogToken =%d, flags:0x%02x, op_ch:%d\n", + (tx) ? "Tx" : "Rx", dialogToken, flags, op_ch); + break; + } + case P2P_INVIT_RESP: + { + struct rtw_wdev_invit_info* invit_info = &pwdev_priv->invit_info; + int status = -1; + int op_ch = 0; + + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len))) + status = *cont; + if ((cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_OPERATING_CH, NULL, &cont_len))) + op_ch = *(cont+4); + + if (invit_info->token != dialogToken) { + rtw_wdev_invit_info_init(invit_info); + } else { + invit_info->token = 0; + invit_info->status = (status ==-1) ? 0xff : status; + invit_info->rsp_op_ch = op_ch; + } + + DBG_8723A("RTW_%s:P2P_INVIT_RESP, dialogToken =%d, status:%d, op_ch:%d\n", + (tx == true)?"Tx":"Rx", dialogToken, status, op_ch); + break; + } + case P2P_DEVDISC_REQ: + DBG_8723A("RTW_%s:P2P_DEVDISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + break; + case P2P_DEVDISC_RESP: + cont = rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_STATUS, NULL, &cont_len); + DBG_8723A("RTW_%s:P2P_DEVDISC_RESP, dialogToken =%d, status:%d\n", (tx == true)?"Tx":"Rx", dialogToken, cont?*cont:-1); + break; + case P2P_PROVISION_DISC_REQ: + { + size_t frame_body_len = len - sizeof(struct ieee80211_hdr_3addr); + u8 *p2p_ie; + uint p2p_ielen = 0; + uint contentlen = 0; + + DBG_8723A("RTW_%s:P2P_PROVISION_DISC_REQ, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + + pwdev_priv->provdisc_req_issued = false; + + p2p_ie = rtw_get_p2p_ie23a(frame_body + _PUBLIC_ACTION_IE_OFFSET_, + frame_body_len - _PUBLIC_ACTION_IE_OFFSET_, + NULL, &p2p_ielen); + if (p2p_ie) { + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, NULL, &contentlen)) + pwdev_priv->provdisc_req_issued = false;/* case: p2p_client join p2p GO */ + else + pwdev_priv->provdisc_req_issued = true;/* case: p2p_devices connection before Nego req. */ + } + } + break; + case P2P_PROVISION_DISC_RESP: + DBG_8723A("RTW_%s:P2P_PROVISION_DISC_RESP, dialogToken =%d\n", (tx == true)?"Tx":"Rx", dialogToken); + break; + default: + DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"Tx":"Rx", OUI_Subtype, dialogToken); + break; + } + + } + + } + else if (category == WLAN_CATEGORY_VENDOR_SPECIFIC) + { + OUI_Subtype = frame_body[5]; + dialogToken = frame_body[6]; + + is_p2p_frame = OUI_Subtype; + + switch (OUI_Subtype) { + case P2P_NOTICE_OF_ABSENCE: + DBG_8723A("RTW_%s:P2P_NOTICE_OF_ABSENCE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + case P2P_PRESENCE_REQUEST: + DBG_8723A("RTW_%s:P2P_PRESENCE_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + case P2P_PRESENCE_RESPONSE: + DBG_8723A("RTW_%s:P2P_PRESENCE_RESPONSE, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + case P2P_GO_DISC_REQUEST: + DBG_8723A("RTW_%s:P2P_GO_DISC_REQUEST, dialogToken =%d\n", (tx == true)?"TX":"RX", dialogToken); + break; + default: + DBG_8723A("RTW_%s:OUI_Subtype =%d, dialogToken =%d\n", (tx == true)?"TX":"RX", OUI_Subtype, dialogToken); + break; + } + + } else { + DBG_8723A("RTW_%s:action frame category =%d\n", (tx == true)?"TX":"RX", category); + } + return is_p2p_frame; +} + +void rtw_init_cfg80211_wifidirect_info(struct rtw_adapter *padapter) +{ + struct cfg80211_wifidirect_info *pcfg80211_wdinfo = &padapter->cfg80211_wdinfo; + + memset(pcfg80211_wdinfo, 0x00, sizeof(struct cfg80211_wifidirect_info)); + + setup_timer(&pcfg80211_wdinfo->remain_on_ch_timer, + ro_ch_timer_process, (unsigned long)padapter); +} + +void p2p_protocol_wk_hdl23a(struct rtw_adapter *padapter, int intCmdType) +{ + switch (intCmdType) { + case P2P_FIND_PHASE_WK: + find_phase_handler(padapter); + break; + case P2P_RESTORE_STATE_WK: + restore_p2p_state_handler(padapter); + break; + case P2P_PRE_TX_PROVDISC_PROCESS_WK: + pre_tx_provdisc_handler(padapter); + break; + case P2P_PRE_TX_INVITEREQ_PROCESS_WK: + pre_tx_invitereq_handler(padapter); + break; + case P2P_PRE_TX_NEGOREQ_PROCESS_WK: + pre_tx_negoreq_handler(padapter); + break; + case P2P_RO_CH_WK: + ro_ch_handler(padapter); + break; + } +} + +#ifdef CONFIG_8723AU_P2P +void process_p2p_ps_ie23a(struct rtw_adapter *padapter, u8 *IEs, u32 IELength) +{ + u8 * ies; + u32 ies_len; + u8 * p2p_ie; + u32 p2p_ielen = 0; + u8 noa_attr[MAX_P2P_IE_LEN] = { 0x00 };/* NoA length should be n*(13) + 2 */ + u32 attr_contentlen = 0; + + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + u8 find_p2p = false, find_p2p_ps = false; + u8 noa_offset, noa_num, noa_index; + + + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + { + return; + } + if (IELength <= _BEACON_IE_OFFSET_) + return; + + ies = IEs + _BEACON_IE_OFFSET_; + ies_len = IELength - _BEACON_IE_OFFSET_; + + p2p_ie = rtw_get_p2p_ie23a(ies, ies_len, NULL, &p2p_ielen); + + while(p2p_ie) + { + find_p2p = true; + /* Get Notice of Absence IE. */ + if (rtw_get_p2p_attr23a_content(p2p_ie, p2p_ielen, P2P_ATTR_NOA, noa_attr, &attr_contentlen)) + { + find_p2p_ps = true; + noa_index = noa_attr[0]; + + if ((pwdinfo->p2p_ps_mode == P2P_PS_NONE) || + (noa_index != pwdinfo->noa_index))/* if index change, driver should reconfigure related setting. */ + { + pwdinfo->noa_index = noa_index; + pwdinfo->opp_ps = noa_attr[1] >> 7; + pwdinfo->ctwindow = noa_attr[1] & 0x7F; + + noa_offset = 2; + noa_num = 0; + /* NoA length should be n*(13) + 2 */ + if (attr_contentlen > 2) + { + while(noa_offset < attr_contentlen) + { + /* memcpy(&wifidirect_info->noa_count[noa_num], &noa_attr[noa_offset], 1); */ + pwdinfo->noa_count[noa_num] = noa_attr[noa_offset]; + noa_offset += 1; + + memcpy(&pwdinfo->noa_duration[noa_num], &noa_attr[noa_offset], 4); + noa_offset += 4; + + memcpy(&pwdinfo->noa_interval[noa_num], &noa_attr[noa_offset], 4); + noa_offset += 4; + + memcpy(&pwdinfo->noa_start_time[noa_num], &noa_attr[noa_offset], 4); + noa_offset += 4; + + noa_num++; + } + } + pwdinfo->noa_num = noa_num; + + if (pwdinfo->opp_ps == 1) + { + pwdinfo->p2p_ps_mode = P2P_PS_CTWINDOW; + /* driver should wait LPS for entering CTWindow */ + if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true) + { + p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1); + } + } + else if (pwdinfo->noa_num > 0) + { + pwdinfo->p2p_ps_mode = P2P_PS_NOA; + p2p_ps_wk_cmd23a(padapter, P2P_PS_ENABLE, 1); + } + else if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) + { + p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1); + } + } + + break; /* find target, just break. */ + } + + /* Get the next P2P IE */ + p2p_ie = rtw_get_p2p_ie23a(p2p_ie+p2p_ielen, ies_len -(p2p_ie -ies + p2p_ielen), NULL, &p2p_ielen); + + } + + if (find_p2p == true) + { + if ((pwdinfo->p2p_ps_mode > P2P_PS_NONE) && (find_p2p_ps == false)) + { + p2p_ps_wk_cmd23a(padapter, P2P_PS_DISABLE, 1); + } + } + + +} + +void p2p_ps_wk_hdl23a(struct rtw_adapter *padapter, u8 p2p_ps_state) +{ + struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + + + /* Pre action for p2p state */ + switch (p2p_ps_state) + { + case P2P_PS_DISABLE: + pwdinfo->p2p_ps_state = p2p_ps_state; + + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + + pwdinfo->noa_index = 0; + pwdinfo->ctwindow = 0; + pwdinfo->opp_ps = 0; + pwdinfo->noa_num = 0; + pwdinfo->p2p_ps_mode = P2P_PS_NONE; + if (padapter->pwrctrlpriv.bFwCurrentInPSMode == true) + { + if (pwrpriv->smart_ps == 0) + { + pwrpriv->smart_ps = 2; + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode); + } + } + break; + case P2P_PS_ENABLE: + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) { + pwdinfo->p2p_ps_state = p2p_ps_state; + + if (pwdinfo->ctwindow > 0) + { + if (pwrpriv->smart_ps != 0) + { + pwrpriv->smart_ps = 0; + DBG_8723A("%s(): Enter CTW, change SmartPS\n", __func__); + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)&padapter->pwrctrlpriv.pwr_mode); + } + } + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + } + break; + case P2P_PS_SCAN: + case P2P_PS_SCAN_DONE: + case P2P_PS_ALLSTASLEEP: + if (pwdinfo->p2p_ps_mode > P2P_PS_NONE) { + pwdinfo->p2p_ps_state = p2p_ps_state; + rtw_hal_set_hwreg23a(padapter, HW_VAR_H2C_FW_P2P_PS_OFFLOAD, (u8 *)(&p2p_ps_state)); + } + break; + default: + break; + } + + +} + +u8 p2p_ps_wk_cmd23a(struct rtw_adapter*padapter, u8 p2p_ps_state, u8 enqueue) +{ + struct cmd_obj *ph2c; + struct drvextra_cmd_parm *pdrvextra_cmd_parm; + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + struct cmd_priv *pcmdpriv = &padapter->cmdpriv; + u8 res = _SUCCESS; + + + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return res; + + if (enqueue) { + ph2c = (struct cmd_obj *)kzalloc(sizeof(struct cmd_obj), + GFP_ATOMIC); + if (!ph2c) { + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm = (struct drvextra_cmd_parm *) + kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC); + if (pdrvextra_cmd_parm == NULL) { + kfree(ph2c); + res = _FAIL; + goto exit; + } + + pdrvextra_cmd_parm->ec_id = P2P_PS_WK_CID; + pdrvextra_cmd_parm->type_size = p2p_ps_state; + pdrvextra_cmd_parm->pbuf = NULL; + + init_h2fwcmd_w_parm_no_rsp(ph2c, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra)); + + res = rtw_enqueue_cmd23a(pcmdpriv, ph2c); + } + else + { + p2p_ps_wk_hdl23a(padapter, p2p_ps_state); + } + +exit: + + + + return res; +} +#endif /* CONFIG_8723AU_P2P */ + +static void reset_ch_sitesurvey_timer_process(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + DBG_8723A("[%s] In\n", __func__); + /* Reset the operation channel information */ + pwdinfo->rx_invitereq_info.operation_ch[0] = 0; + pwdinfo->rx_invitereq_info.scan_op_ch_only = 0; +} + +static void reset_ch_sitesurvey_timer_process2(unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + DBG_8723A("[%s] In\n", __func__); + /* Reset the operation channel information */ + pwdinfo->p2p_info.operation_ch[0] = 0; + pwdinfo->p2p_info.scan_op_ch_only = 0; +} + +static void restore_p2p_state_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + p2p_protocol_wk_cmd23a(adapter, P2P_RESTORE_STATE_WK); +} + +static void pre_tx_scan_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + struct mlme_priv *pmlmepriv = &adapter->mlmepriv; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + spin_lock_bh(&pmlmepriv->lock); + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_PROVISION_DIS_REQ)) + { + if (true == pwdinfo->tx_prov_disc_info.benable) /* the provision discovery request frame is trigger to send or not */ + { + p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_PROVDISC_PROCESS_WK); + /* issue23a_probereq_p2p(adapter, NULL); */ + /* _set_timer(&pwdinfo->pre_tx_scan_timer, P2P_TX_PRESCAN_TIMEOUT); */ + } + } + else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_GONEGO_ING)) + { + if (true == pwdinfo->nego_req_info.benable) + { + p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_NEGOREQ_PROCESS_WK); + } + } + else if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_TX_INVITE_REQ)) + { + if (true == pwdinfo->invitereq_info.benable) + { + p2p_protocol_wk_cmd23a(adapter, P2P_PRE_TX_INVITEREQ_PROCESS_WK); + } + } + else + { + DBG_8723A("[%s] p2p_state is %d, ignore!!\n", __func__, rtw_p2p_state(pwdinfo)); + } + + spin_unlock_bh(&pmlmepriv->lock); +} + +static void find_phase_timer_process (unsigned long data) +{ + struct rtw_adapter *adapter = (struct rtw_adapter *)data; + struct wifidirect_info *pwdinfo = &adapter->wdinfo; + + if (rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + return; + + adapter->wdinfo.find_phase_state_exchange_cnt++; + + p2p_protocol_wk_cmd23a(adapter, P2P_FIND_PHASE_WK); +} + +void reset_global_wifidirect_info23a(struct rtw_adapter *padapter) +{ + struct wifidirect_info *pwdinfo; + + pwdinfo = &padapter->wdinfo; + pwdinfo->persistent_supported = 0; + pwdinfo->session_available = true; + pwdinfo->wfd_tdls_enable = 0; + pwdinfo->wfd_tdls_weaksec = 0; +} + +#ifdef CONFIG_8723AU_P2P +int rtw_init_wifi_display_info(struct rtw_adapter* padapter) +{ + int res = _SUCCESS; + struct wifi_display_info *pwfd_info = &padapter->wfd_info; + + /* Used in P2P and TDLS */ + pwfd_info->rtsp_ctrlport = 554; + pwfd_info->peer_rtsp_ctrlport = 0; /* Reset to 0 */ + pwfd_info->wfd_enable = false; + pwfd_info->wfd_device_type = WFD_DEVINFO_PSINK; + pwfd_info->scan_result_type = SCAN_RESULT_P2P_ONLY; + + /* Used in P2P */ + pwfd_info->peer_session_avail = true; + pwfd_info->wfd_pc = false; + + /* Used in TDLS */ + memset(pwfd_info->ip_address, 0x00, 4); + memset(pwfd_info->peer_ip_address, 0x00, 4); + return res; +} +#endif /* CONFIG_8723AU_P2P */ + +void rtw_init_wifidirect_timers23a(struct rtw_adapter* padapter) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + setup_timer(&pwdinfo->find_phase_timer, find_phase_timer_process, + (unsigned long)padapter); + setup_timer(&pwdinfo->restore_p2p_state_timer, + restore_p2p_state_timer_process, (unsigned long)padapter); + setup_timer(&pwdinfo->pre_tx_scan_timer, pre_tx_scan_timer_process, + (unsigned long)padapter); + setup_timer(&pwdinfo->reset_ch_sitesurvey, + reset_ch_sitesurvey_timer_process, (unsigned long)padapter); + setup_timer(&pwdinfo->reset_ch_sitesurvey2, + reset_ch_sitesurvey_timer_process2, + (unsigned long)padapter); +} + +void rtw_init_wifidirect_addrs23a(struct rtw_adapter* padapter, u8 *dev_addr, u8 *iface_addr) +{ +#ifdef CONFIG_8723AU_P2P + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + + /*init device&interface address */ + if (dev_addr) { + memcpy(pwdinfo->device_addr, dev_addr, ETH_ALEN); + } + if (iface_addr) { + memcpy(pwdinfo->interface_addr, iface_addr, ETH_ALEN); + } +#endif +} + +void init_wifidirect_info23a(struct rtw_adapter *padapter, enum P2P_ROLE role) +{ + struct wifidirect_info *pwdinfo; +#ifdef CONFIG_8723AU_P2P + struct wifi_display_info *pwfd_info = &padapter->wfd_info; +#endif + + pwdinfo = &padapter->wdinfo; + + pwdinfo->padapter = padapter; + + /* 1, 6, 11 are the social channel defined in the WiFi Direct specification. */ + pwdinfo->social_chan[0] = 1; + pwdinfo->social_chan[1] = 6; + pwdinfo->social_chan[2] = 11; + pwdinfo->social_chan[3] = 0; /* channel 0 for scanning ending in site survey function. */ + + /* Use the channel 11 as the listen channel */ + pwdinfo->listen_channel = 11; + + if (role == P2P_ROLE_DEVICE) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DEVICE); + rtw_p2p_set_state(pwdinfo, P2P_STATE_LISTEN); + pwdinfo->intent = 1; + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_LISTEN); + } + else if (role == P2P_ROLE_CLIENT) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_CLIENT); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + pwdinfo->intent = 1; + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + } + else if (role == P2P_ROLE_GO) + { + rtw_p2p_set_role(pwdinfo, P2P_ROLE_GO); + rtw_p2p_set_state(pwdinfo, P2P_STATE_GONEGO_OK); + pwdinfo->intent = 15; + rtw_p2p_set_pre_state(pwdinfo, P2P_STATE_GONEGO_OK); + } + +/* Use the OFDM rate in the P2P probe response frame. (6(B), 9(B), 12, 18, 24, 36, 48, 54) */ + pwdinfo->support_rate[0] = 0x8c; /* 6(B) */ + pwdinfo->support_rate[1] = 0x92; /* 9(B) */ + pwdinfo->support_rate[2] = 0x18; /* 12 */ + pwdinfo->support_rate[3] = 0x24; /* 18 */ + pwdinfo->support_rate[4] = 0x30; /* 24 */ + pwdinfo->support_rate[5] = 0x48; /* 36 */ + pwdinfo->support_rate[6] = 0x60; /* 48 */ + pwdinfo->support_rate[7] = 0x6c; /* 54 */ + + memcpy((void*) pwdinfo->p2p_wildcard_ssid, "DIRECT-", 7); + + memset(pwdinfo->device_name, 0x00, WPS_MAX_DEVICE_NAME_LEN); + pwdinfo->device_name_len = 0; + + memset(&pwdinfo->invitereq_info, 0x00, sizeof(struct tx_invite_req_info)); + pwdinfo->invitereq_info.token = 3; /* Token used for P2P invitation request frame. */ + + memset(&pwdinfo->inviteresp_info, 0x00, sizeof(struct tx_invite_resp_info)); + pwdinfo->inviteresp_info.token = 0; + + pwdinfo->profileindex = 0; + memset(&pwdinfo->profileinfo[ 0 ], 0x00, sizeof(struct profile_info) * P2P_MAX_PERSISTENT_GROUP_NUM); + + rtw_p2p_findphase_ex_set(pwdinfo, P2P_FINDPHASE_EX_NONE); + + pwdinfo->listen_dwell = (u8) ((jiffies % 3) + 1); + /* DBG_8723A("[%s] listen_dwell time is %d00ms\n", __func__, pwdinfo->listen_dwell); */ + + memset(&pwdinfo->tx_prov_disc_info, 0x00, sizeof(struct tx_provdisc_req_info)); + pwdinfo->tx_prov_disc_info.wps_config_method_request = WPS_CM_NONE; + + memset(&pwdinfo->nego_req_info, 0x00, sizeof(struct tx_nego_req_info)); + + pwdinfo->device_password_id_for_nego = WPS_DPID_PBC; + pwdinfo->negotiation_dialog_token = 1; + + memset(pwdinfo->nego_ssid, 0x00, IEEE80211_MAX_SSID_LEN); + pwdinfo->nego_ssidlen = 0; + + pwdinfo->ui_got_wps_info = P2P_NO_WPSINFO; +#ifdef CONFIG_8723AU_P2P + pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC; + pwdinfo->wfd_info = pwfd_info; +#else + pwdinfo->supported_wps_cm = WPS_CONFIG_METHOD_DISPLAY | WPS_CONFIG_METHOD_PBC | WPS_CONFIG_METHOD_KEYPAD; +#endif /* CONFIG_8723AU_P2P */ + pwdinfo->channel_list_attr_len = 0; + memset(pwdinfo->channel_list_attr, 0x00, 100); + + memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, 0x00, 4); + memset(pwdinfo->rx_prov_disc_info.strconfig_method_desc_of_prov_disc_req, '0', 3); + memset(&pwdinfo->groupid_info, 0x00, sizeof(struct group_id_info)); + pwdinfo->wfd_tdls_enable = 0; + memset(pwdinfo->p2p_peer_interface_addr, 0x00, ETH_ALEN); + memset(pwdinfo->p2p_peer_device_addr, 0x00, ETH_ALEN); + + pwdinfo->rx_invitereq_info.operation_ch[0] = 0; + pwdinfo->rx_invitereq_info.operation_ch[1] = 0; /* Used to indicate the scan end in site survey function */ + pwdinfo->rx_invitereq_info.scan_op_ch_only = 0; + pwdinfo->p2p_info.operation_ch[0] = 0; + pwdinfo->p2p_info.operation_ch[1] = 0; /* Used to indicate the scan end in site survey function */ + pwdinfo->p2p_info.scan_op_ch_only = 0; +} + +int rtw_p2p_enable23a(struct rtw_adapter *padapter, enum P2P_ROLE role) +{ + struct wifidirect_info *pwdinfo = &padapter->wdinfo; + int ret = _SUCCESS; + + if (role == P2P_ROLE_DEVICE || role == P2P_ROLE_CLIENT || + role == P2P_ROLE_GO) { + /* leave IPS/Autosuspend */ + if (_FAIL == rtw_pwr_wakeup(padapter)) { + ret = _FAIL; + goto exit; + } + + /* Added by Albert 2011/03/22 */ + /* In the P2P mode, the driver should not support the b mode. */ + /* So, the Tx packet shouldn't use the CCK rate */ + update_tx_basic_rate23a(padapter, WIRELESS_11AGN); + + /* Enable P2P function */ + init_wifidirect_info23a(padapter, role); + + rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, true); + #ifdef CONFIG_8723AU_P2P + rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, true); + #endif + + } + else if (role == P2P_ROLE_DISABLE) + { + if (_FAIL == rtw_pwr_wakeup(padapter)) { + ret = _FAIL; + goto exit; + } + + /* Disable P2P function */ + if (!rtw_p2p_chk_state(pwdinfo, P2P_STATE_NONE)) + { + del_timer_sync(&pwdinfo->find_phase_timer); + del_timer_sync(&pwdinfo->restore_p2p_state_timer); + del_timer_sync(&pwdinfo->pre_tx_scan_timer); + del_timer_sync(&pwdinfo->reset_ch_sitesurvey); + del_timer_sync(&pwdinfo->reset_ch_sitesurvey2); + reset_ch_sitesurvey_timer_process((unsigned long)padapter); + reset_ch_sitesurvey_timer_process2((unsigned long)padapter); + rtw_p2p_set_state(pwdinfo, P2P_STATE_NONE); + rtw_p2p_set_role(pwdinfo, P2P_ROLE_DISABLE); + memset(&pwdinfo->rx_prov_disc_info, 0x00, sizeof(struct rx_provdisc_req_info)); + } + + rtw_hal_set_odm_var23a(padapter, HAL_ODM_P2P_STATE, NULL, false); + #ifdef CONFIG_8723AU_P2P + rtw_hal_set_odm_var23a(padapter, HAL_ODM_WIFI_DISPLAY_STATE, NULL, false); + #endif + + /* Restore to initial setting. */ + update_tx_basic_rate23a(padapter, padapter->registrypriv.wireless_mode); + } + +exit: + return ret; +} + +#endif /* CONFIG_8723AU_P2P */ |