u8 *rtw_get_wps_attr_content_ex(u8 *wps_ie, uint wps_ielen, u16 target_attr_id ,u8 *attr_content, uint *attr_contentlen)
{
u8 *pattr_content = NULL;
uint cnt = 0;
u8 wps_oui[4]={0x00,0x50,0xF2,0x04};
u16 attr_id;
if ( ( wps_ie[ 0 ] != _VENDOR_SPECIFIC_IE_ ) ||
( _rtw_memcmp( wps_ie + 2, wps_oui , 4 ) != _TRUE ) )
{
return( pattr_content );
}
// 1 ( WPS IE ) + 1 ( Length ) + 4 ( WPS OUI )
cnt = 6;
while( cnt < wps_ielen )
{
// 2 -> the length of WPS attribute type field.
//u16 attrlen = be16_to_cpu(*(u16*)(wps_ie + cnt + 2 ));
u16 attrlen = RTW_GET_BE16(wps_ie + cnt + 2);
//attr_id = be16_to_cpu( *(u16*) ( wps_ie + cnt ) );
attr_id = RTW_GET_BE16(wps_ie + cnt);
if( attr_id == target_attr_id )
{
// 3 -> 1 byte for attribute ID field, 2 bytes for length field
if(attr_content)
_rtw_memcpy( attr_content, &wps_ie[ cnt + 4 ], attrlen );
pattr_content = &wps_ie[ cnt + 4 ];
if(attr_contentlen)
*attr_contentlen = attrlen;
cnt += attrlen + 4;
break;
}
else
{
cnt += attrlen + 4; //goto next
}
}
return pattr_content;
}
/**
* rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
* @wps_ie: Address of WPS IE to search
* @wps_ielen: Length limit from wps_ie
* @target_attr_id: The attribute ID of WPS attribute to search
* @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
* @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
*
* Returns: the address of the specific WPS attribute found, or NULL
*/
u8 *rtw_get_wps_attr(u8 *wps_ie, uint wps_ielen, u16 target_attr_id , u8 *buf_attr, u32 *len_attr)
{
u8 *attr_ptr = NULL;
u8 * target_attr_ptr = NULL;
u8 wps_oui[4]={0x00, 0x50, 0xF2, 0x04};
if (len_attr)
*len_attr = 0;
if ((wps_ie[0] != _VENDOR_SPECIFIC_IE_) ||
(memcmp(wps_ie + 2, wps_oui , 4)))
{
return attr_ptr;
}
/* 6 = 1(Element ID) + 1(Length) + 4(WPS OUI) */
attr_ptr = wps_ie + 6; /* goto first attr */
while (attr_ptr - wps_ie < wps_ielen)
{
/* 4 = 2(Attribute ID) + 2(Length) */
u16 attr_id = RTW_GET_BE16(attr_ptr);
u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
u16 attr_len = attr_data_len + 4;
/* DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
if (attr_id == target_attr_id)
{
target_attr_ptr = attr_ptr;
if (buf_attr)
memcpy(buf_attr, attr_ptr, attr_len);
if (len_attr)
*len_attr = attr_len;
break;
}
else
{
attr_ptr += attr_len; /* goto next */
}
}
return target_attr_ptr;
}
/**
* rtw_get_wps_attr - Search a specific WPS attribute from a given WPS IE
* @wps_ie: Address of WPS IE to search
* @wps_ielen: Length limit from wps_ie
* @target_attr_id: The attribute ID of WPS attribute to search
* @buf_attr: If not NULL and the WPS attribute is found, WPS attribute will be copied to the buf starting from buf_attr
* @len_attr: If not NULL and the WPS attribute is found, will set to the length of the entire WPS attribute
*
* Returns: the address of the specific WPS attribute found, or NULL
*/
uint8_t *rtw_get_wps_attr(uint8_t *wps_ie, uint wps_ielen, u16 target_attr_id ,uint8_t *buf_attr, uint32_t *len_attr)
{
uint8_t *attr_ptr = NULL;
uint8_t * target_attr_ptr = NULL;
uint8_t wps_oui[4]={0x00,0x50,0xF2,0x04};
if(len_attr)
*len_attr = 0;
if ( ( wps_ie[0] != _VENDOR_SPECIFIC_IE_ ) ||
( _rtw_memcmp( wps_ie + 2, wps_oui , 4 ) != true ) )
{
return attr_ptr;
}
// 6 = 1(Element ID) + 1(Length) + 4(WPS OUI)
attr_ptr = wps_ie + 6; //goto first attr
while(attr_ptr - wps_ie < wps_ielen)
{
// 4 = 2(Attribute ID) + 2(Length)
u16 attr_id = RTW_GET_BE16(attr_ptr);
u16 attr_data_len = RTW_GET_BE16(attr_ptr + 2);
u16 attr_len = attr_data_len + 4;
//DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __FUNCTION__, attr_ptr, attr_id, attr_data_len);
if( attr_id == target_attr_id )
{
target_attr_ptr = attr_ptr;
if(buf_attr)
memcpy(buf_attr, attr_ptr, attr_len);
if(len_attr)
*len_attr = attr_len;
break;
}
else
{
attr_ptr += attr_len; //goto next
}
}
return target_attr_ptr;
}
_pkt *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8 *pdata)
{
u16 eth_type;
u8 *data_ptr;
_pkt *sub_skb;
struct rx_pkt_attrib *pattrib;
pattrib = &prframe->u.hdr.attrib;
#ifdef CONFIG_SKB_COPY
sub_skb = rtw_skb_alloc(nSubframe_Length + 12);
if(sub_skb)
{
skb_reserve(sub_skb, 12);
data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
_rtw_memcpy(data_ptr, (pdata + ETH_HLEN), nSubframe_Length);
}
else
#endif // CONFIG_SKB_COPY
{
sub_skb = rtw_skb_clone(prframe->u.hdr.pkt);
if(sub_skb)
{
sub_skb->data = pdata + ETH_HLEN;
sub_skb->len = nSubframe_Length;
skb_set_tail_pointer(sub_skb, nSubframe_Length);
}
else
{
DBG_871X("%s(): rtw_skb_clone() Fail!!!\n",__FUNCTION__);
return NULL;
}
}
eth_type = RTW_GET_BE16(&sub_skb->data[6]);
if (sub_skb->len >= 8 &&
((_rtw_memcmp(sub_skb->data, rtw_rfc1042_header, SNAP_SIZE) &&
eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
_rtw_memcmp(sub_skb->data, rtw_bridge_tunnel_header, SNAP_SIZE) )) {
/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
skb_pull(sub_skb, SNAP_SIZE);
_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
} else {
u16 len;
/* Leave Ethernet header part of hdr and full payload */
len = htons(sub_skb->len);
_rtw_memcpy(skb_push(sub_skb, 2), &len, 2);
_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->src, ETH_ALEN);
_rtw_memcpy(skb_push(sub_skb, ETH_ALEN), pattrib->dst, ETH_ALEN);
}
return sub_skb;
}
void dump_wps_ie(u8 *ie, u32 ie_len) {
u8* pos = (u8*)ie;
u16 id;
u16 len;
u8 *wps_ie;
uint wps_ielen;
wps_ie = rtw_get_wps_ie(ie, ie_len, NULL, &wps_ielen);
if(wps_ie != ie || wps_ielen == 0)
return;
pos+=6;
while(pos-ie < ie_len){
id = RTW_GET_BE16(pos);
len = RTW_GET_BE16(pos + 2);
DBG_871X("%s ID:0x%04x, LEN:%u\n", __FUNCTION__, id, len);
pos+=(4+len);
}
}
// attr_content: The output buffer, contains the "body field" of WFD attribute.
// attr_contentlen: The data length of the "body field" of WFD attribute.
int rtw_get_wfd_attr_content(u8 *wfd_ie, uint wfd_ielen, u8 target_attr_id ,u8 *attr_content, uint *attr_contentlen)
{
int match;
uint cnt = 0;
u8 attr_id, wfd_oui[4]={0x50,0x6F,0x9A,0x0A};
match=_FALSE;
if ( ( wfd_ie[ 0 ] != _VENDOR_SPECIFIC_IE_ ) ||
( _rtw_memcmp( wfd_ie + 2, wfd_oui , 4 ) != _TRUE ) )
{
return( match );
}
// 1 ( WFD IE ) + 1 ( Length ) + 3 ( OUI ) + 1 ( OUI Type )
cnt = 6;
while( cnt < wfd_ielen )
{
u16 attrlen = RTW_GET_BE16(wfd_ie + cnt + 1);
attr_id = wfd_ie[cnt];
if( attr_id == target_attr_id )
{
// 3 -> 1 byte for attribute ID field, 2 bytes for length field
if(attr_content)
_rtw_memcpy( attr_content, &wfd_ie[ cnt + 3 ], attrlen );
if(attr_contentlen)
*attr_contentlen = attrlen;
cnt += attrlen + 3;
match = _TRUE;
break;
}
else
{
cnt += attrlen + 3; //goto next
}
}
return match;
}
