static uint rtw_p2p_attr_remove(u8 *ie, uint ielen_ori, u8 attr_id)
{
u8 *target_attr;
uint target_attr_clen;
uint ielen = ielen_ori;
int index=0;
while(1) {
target_attr=rtw_get_p2p_attr_content(ie, ielen, attr_id, NULL, &target_attr_clen);
if(target_attr && target_attr_clen)
{
u8 *next_attr = target_attr+target_attr_clen+3;
uint remain_len = ielen-(next_attr-ie);
//dump_ie(ie, ielen);
//DBG_871X("[%d] ie:%p, ielen:%u\n"
// "target_attr:%p, target_attr_clen:%u\n"
// "next_attr:%p, remain_len:%u\n"
// , index++
// , ie, ielen
// , target_attr, target_attr_clen
// , next_attr, remain_len
//);
_rtw_memset(target_attr, 0, target_attr_clen+3);
_rtw_memcpy(target_attr, next_attr, remain_len);
_rtw_memset(target_attr+remain_len, 0, ielen-(target_attr_clen+3));
*(ie+1) -= target_attr_clen+3;
ielen-=target_attr_clen+3;
}
else
{
//if(index>0)
// dump_ie(ie, ielen);
break;
}
}
return ielen;
}
u32 process_assoc_req_p2p_ie(struct wifidirect_info *pwdinfo, u8 *p2p_ie, uint p2p_ielen, struct sta_info *psta)
{
u8 status_code = P2P_STATUS_SUCCESS;
u8 *pbuf, *pattr_content=NULL;
u32 attr_contentlen = 0;
u16 cap_attr=0;
if(pwdinfo->role != P2P_ROLE_GO)
return P2P_STATUS_FAIL_REQUEST_UNABLE;
//Check P2P Capability ATTR
if(rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_CAPABILITY, (u8*)&cap_attr, (uint*)&attr_contentlen)==_FALSE)
return P2P_STATUS_FAIL_INVALID_PARAM;
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_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO, NULL, (uint*)&attr_contentlen)==_TRUE)
{
pattr_content = pbuf = rtw_zmalloc(attr_contentlen);
if(pattr_content)
{
u8 num_of_secdev_type;
u16 dev_name_len;
rtw_get_p2p_attr_content(p2p_ie, p2p_ielen, P2P_ATTR_DEVICE_INFO , pattr_content, (uint*)&attr_contentlen);
_rtw_memcpy(psta->dev_addr, pattr_content, ETH_ALEN);//P2P Device Address
pattr_content += ETH_ALEN;
_rtw_memcpy(&psta->config_methods, pattr_content, 2);//Config Methods
psta->config_methods = be16_to_cpu(psta->config_methods);
pattr_content += 2;
_rtw_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);
_rtw_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;
_rtw_memcpy(psta->dev_name, pattr_content+4, psta->dev_name_len);
}
rtw_mfree(pbuf, attr_contentlen);
}
}
else
{
status_code = P2P_STATUS_FAIL_INVALID_PARAM;
}
return status_code;
}
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
{