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 {