void rtw_WLAN_BSSID_EX_remove_p2p_attr(WLAN_BSSID_EX *bss_ex, u8 attr_id)
{
u8 *p2p_ie;
uint p2p_ielen, p2p_ielen_ori;
int cnt;
if( (p2p_ie=rtw_get_p2p_ie(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_, NULL, &p2p_ielen_ori)) )
{
#if 0
if(rtw_get_p2p_attr(p2p_ie, p2p_ielen_ori, attr_id, NULL, NULL)) {
DBG_871X("rtw_get_p2p_attr: GOT P2P_ATTR:%u!!!!!!!!\n", attr_id);
dump_ies(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_);
}
#endif
p2p_ielen=rtw_p2p_attr_remove(p2p_ie, p2p_ielen_ori, attr_id);
if(p2p_ielen != p2p_ielen_ori) {
u8 *next_ie_ori = p2p_ie+p2p_ielen_ori;
u8 *next_ie = p2p_ie+p2p_ielen;
uint remain_len = bss_ex->IELength-(next_ie_ori-bss_ex->IEs);
_rtw_memcpy(next_ie, next_ie_ori, remain_len);
_rtw_memset(next_ie+remain_len, 0, p2p_ielen_ori-p2p_ielen);
bss_ex->IELength -= p2p_ielen_ori-p2p_ielen;
#if 0
DBG_871X("remove P2P_ATTR:%u!\n", attr_id);
dump_ies(bss_ex->IEs+_FIXED_IE_LENGTH_, bss_ex->IELength-_FIXED_IE_LENGTH_);
#endif
}
}
}
u8 *rtw_get_p2p_ie_from_scan_queue(u8 *in_ie, int in_len, u8 *p2p_ie, uint *p2p_ielen, u8 frame_type)
{
u8* p2p = NULL;
DBG_871X( "[%s] frame_type = %d\n", __FUNCTION__, frame_type );
switch( frame_type )
{
case 1:
case 3:
{ // Beacon or Probe Response
p2p = rtw_get_p2p_ie(in_ie + _PROBERSP_IE_OFFSET_, in_len - _PROBERSP_IE_OFFSET_, p2p_ie, p2p_ielen);
break;
}
case 2:
{ // Probe Request
p2p = rtw_get_p2p_ie(in_ie + _PROBEREQ_IE_OFFSET_ , in_len - _PROBEREQ_IE_OFFSET_ , p2p_ie, p2p_ielen);
break;
}
}
return p2p;
}
void dump_p2p_attr(u8 *ie, u32 ie_len) {
u8* pos = (u8*)ie;
u8 id;
u16 len;
uint is_p2p_ie;
rtw_get_p2p_ie(ie, ie_len, NULL, &is_p2p_ie);
if(!is_p2p_ie)
return;
pos+=6;
while(pos-ie<=ie_len){
id = *pos;
len = RTW_GET_LE16(pos+1);
DBG_871X("%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
pos+=(3+len);
}
}
void dump_p2p_ie(u8 *ie, u32 ie_len) {
u8* pos = (u8*)ie;
u8 id;
u16 len;
u8 *p2p_ie;
uint p2p_ielen;
p2p_ie = rtw_get_p2p_ie(ie, ie_len, NULL, &p2p_ielen);
if(p2p_ie != ie || p2p_ielen == 0)
return;
pos+=6;
while(pos-ie < ie_len){
id = *pos;
len = RTW_GET_LE16(pos+1);
DBG_871X("%s ID:%u, LEN:%u\n", __FUNCTION__, id, len);
pos+=(3+len);
}
}
u32 process_probe_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
{
u8 *p;
u32 ret=_FALSE;
u8 p2pie[ MAX_P2P_IE_LEN ] = { 0xFF };
u32 p2pielen = 0;
int ssid_len=0, rate_cnt = 0;
p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SUPPORTEDRATES_IE_, (int *)&rate_cnt,
len - WLAN_HDR_A3_LEN - _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_dispatcher function )
// 5. Requested Device Type in WSC IE. (Todo)
// 6. Device ID attribute in P2P IE. (Todo)
p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, (int *)&ssid_len,
len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
if((pwdinfo->role == P2P_ROLE_DEVICE) || (pwdinfo->role == P2P_ROLE_GO))
{
if(rtw_get_p2p_ie( pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_ , len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_ , p2pie, &p2pielen))
{
DBG_871X( "[%s] Got P2P IE \n", __FUNCTION__ );
if ( (p != NULL) && _rtw_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
DBG_871X( "[%s] P2P SSID Match!\n", __FUNCTION__ );
ret = _TRUE;
}
}
else
{
//non -p2p device
}
}
return ret;
}
u32 process_p2p_devdisc_req(struct wifidirect_info *pwdinfo, u8 *pframe, uint len)
{
u8 *frame_body;
u8 status, dialogToken;
struct sta_info *psta = NULL;
_adapter *padapter = pwdinfo->padapter;
struct sta_priv *pstapriv = &padapter->stapriv;
u8 p2p_ie[ MAX_P2P_IE_LEN ] = { 0xFF };
u32 p2p_ielen = 0;
frame_body = (unsigned char *)(pframe + sizeof(struct ieee80211_hdr_3addr));
dialogToken = frame_body[7];
status = P2P_STATUS_FAIL_UNKNOWN_P2PGROUP;
if ( rtw_get_p2p_ie( frame_body + _PUBLIC_ACTION_IE_OFFSET_, len - _PUBLIC_ACTION_IE_OFFSET_, p2p_ie, &p2p_ielen ) )
{
u8 groupid[ 38 ] = { 0x00 };
u8 dev_addr[ETH_ALEN] = { 0x00 };
u32 attr_contentlen = 0;
if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_GROUP_ID, groupid, &attr_contentlen))
{
if(_rtw_memcmp(pwdinfo->device_addr, groupid, ETH_ALEN) &&
_rtw_memcmp(pwdinfo->p2p_group_ssid, groupid+ETH_ALEN, pwdinfo->p2p_group_ssid_len))
{
attr_contentlen=0;
if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_ID, dev_addr, &attr_contentlen))
{
_list *phead, *plist;
phead = &pstapriv->asoc_list;
plist = get_next(phead);
//look up sta asoc_queue
while ((rtw_end_of_queue_search(phead, plist)) == _FALSE)
{
psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
plist = get_next(plist);
if(psta->is_p2p_device && (psta->dev_cap&P2P_DEVCAP_CLIENT_DISCOVERABILITY) &&
_rtw_memcmp(psta->dev_addr, dev_addr, ETH_ALEN))
{
//issue GO Discoverability Request
issue_group_disc_req(pwdinfo, psta->hwaddr);
status = P2P_STATUS_SUCCESS;
break;
}
else
{
status = P2P_STATUS_FAIL_INFO_UNAVAILABLE;
}
}
}
else
{
