/**
* rtw_get_p2p_attr - Search a specific P2P attribute from a given P2P IE
* @p2p_ie: Address of P2P IE to search
* @p2p_ielen: Length limit from p2p_ie
* @target_attr_id: The attribute ID of P2P attribute to search
* @buf_attr: If not NULL and the P2P attribute is found, P2P attribute will be copied to the buf starting from buf_attr
* @len_attr: If not NULL and the P2P attribute is found, will set to the length of the entire P2P attribute
*
* Returns: the address of the specific WPS attribute found, or NULL
*/
u8 *rtw_get_p2p_attr(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *buf_attr, u32 *len_attr)
{
u8 *attr_ptr = NULL;
u8 *target_attr_ptr = NULL;
u8 p2p_oui[4]={0x50,0x6F,0x9A,0x09};
if(len_attr)
*len_attr = 0;
if ( !p2p_ie || ( p2p_ie[0] != _VENDOR_SPECIFIC_IE_ ) ||
( _rtw_memcmp( p2p_ie + 2, p2p_oui , 4 ) != _TRUE ) )
{
return attr_ptr;
}
// 6 = 1(Element ID) + 1(Length) + 3 (OUI) + 1(OUI Type)
attr_ptr = p2p_ie + 6; //goto first attr
while(attr_ptr - p2p_ie < p2p_ielen)
{
// 3 = 1(Attribute ID) + 2(Length)
u8 attr_id = *attr_ptr;
u16 attr_data_len = RTW_GET_LE16(attr_ptr + 1);
u16 attr_len = attr_data_len + 3;
//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)
_rtw_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;
}
// 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 = le16_to_cpu(*(u16*)(p2p_ie + cnt + 1 ));
u16 attrlen = RTW_GET_LE16(wfd_ie + cnt + 1);
attr_id = wfd_ie[cnt];
if( attr_id == target_attr_id )
{
// 2 -> 1 byte for attribute ID field, 1 bytes for length field
if(attr_content)
_rtw_memcpy( attr_content, &wfd_ie[ cnt + 2 ], attrlen );
if(attr_contentlen)
*attr_contentlen = attrlen;
cnt += attrlen + 2;
match = _TRUE;
break;
}
else
{
cnt += attrlen + 2; //goto next
}
}
return match;
}
// attr_content: The output buffer, contains the "body field" of P2P attribute.
// attr_contentlen: The data length of the "body field" of P2P attribute.
u8 *rtw_get_p2p_attr_content(u8 *p2p_ie, uint p2p_ielen, u8 target_attr_id ,u8 *attr_content, uint *attr_contentlen)
{
uint cnt = 0;
u8 *p2p_attr_ptr = NULL;
u8 attr_id, p2p_oui[4]={0x50,0x6F,0x9A,0x09};
if(attr_contentlen)
*attr_contentlen = 0;
if ( ( p2p_ie[ 0 ] != _VENDOR_SPECIFIC_IE_ ) ||
( _rtw_memcmp( p2p_ie + 2, p2p_oui , 4 ) != _TRUE ) )
{
return p2p_attr_ptr;
}
// 1 ( P2P IE ) + 1 ( Length ) + 3 ( OUI ) + 1 ( OUI Type )
cnt = 6;
while( cnt < p2p_ielen )
{
//u16 attrlen = le16_to_cpu(*(u16*)(p2p_ie + cnt + 1 ));
u16 attrlen = RTW_GET_LE16(p2p_ie + cnt + 1);
attr_id = p2p_ie[cnt];
if( attr_id == target_attr_id )
{
p2p_attr_ptr = p2p_ie + cnt;
// 3 -> 1 byte for attribute ID field, 2 bytes for length field
if(attr_content)
_rtw_memcpy( attr_content, &p2p_ie[ cnt + 3 ], attrlen );
if(attr_contentlen)
*attr_contentlen = attrlen;
break;
}
else
{
cnt += attrlen + 3; //goto next
}
}
return p2p_attr_ptr;
}
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);
}
}
int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
{
int i, ret = _SUCCESS;
int left, count;
u8 *pos;
u8 SUITE_1X[4] = {0x00, 0x0f, 0xac, 0x01};
if (rsn_ie_len <= 0) {
/* No RSN IE - fail silently */
return _FAIL;
}
if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
{
return _FAIL;
}
pos = rsn_ie;
pos += 4;
left = rsn_ie_len - 4;
/* group_cipher */
if (left >= RSN_SELECTOR_LEN) {
*group_cipher = rtw_get_wpa2_cipher_suite(pos);
pos += RSN_SELECTOR_LEN;
left -= RSN_SELECTOR_LEN;
} else if (left > 0) {
RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
return _FAIL;
}
/* pairwise_cipher */
if (left >= 2)
{
/* count = le16_to_cpu(*(u16*)pos); */
count = RTW_GET_LE16(pos);
pos += 2;
left -= 2;
if (count == 0 || left < count * RSN_SELECTOR_LEN) {
RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
"count %u left %u", __func__, count, left));
return _FAIL;
}
for (i = 0; i < count; i++)
{
*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
pos += RSN_SELECTOR_LEN;
left -= RSN_SELECTOR_LEN;
}
}
else if (left == 1)
{
RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)", __func__));
return _FAIL;
}
if (is_8021x) {
if (left >= 6) {
pos += 2;
if (!memcmp(pos, SUITE_1X, 4)) {
RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s (): there has 802.1x auth\n", __func__));
*is_8021x = 1;
}
}
}
return ret;
}
int rtw_parse_wpa_ie(u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
{
int i, ret=_SUCCESS;
int left, count;
u8 *pos;
u8 SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
if (wpa_ie_len <= 0) {
/* No WPA IE - fail silently */
return _FAIL;
}
if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (u8)(wpa_ie_len - 2)) ||
(_rtw_memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != _TRUE) )
{
return _FAIL;
}
pos = wpa_ie;
pos += 8;
left = wpa_ie_len - 8;
//group_cipher
if (left >= WPA_SELECTOR_LEN) {
*group_cipher = rtw_get_wpa_cipher_suite(pos);
pos += WPA_SELECTOR_LEN;
left -= WPA_SELECTOR_LEN;
}
else if (left > 0)
{
RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie length mismatch, %u too much", __FUNCTION__, left));
return _FAIL;
}
//pairwise_cipher
if (left >= 2)
{
//count = le16_to_cpu(*(u16*)pos);
count = RTW_GET_LE16(pos);
pos += 2;
left -= 2;
if (count == 0 || left < count * WPA_SELECTOR_LEN) {
RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie count botch (pairwise), "
"count %u left %u", __FUNCTION__, count, left));
return _FAIL;
}
for (i = 0; i < count; i++)
{
*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
pos += WPA_SELECTOR_LEN;
left -= WPA_SELECTOR_LEN;
}
}
else if (left == 1)
{
RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie too short (for key mgmt)", __FUNCTION__));
return _FAIL;
}
if (is_8021x) {
if (left >= 6) {
pos += 2;
if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,("%s : there has 802.1x auth\n", __FUNCTION__));
*is_8021x = 1;
}
}
}
return ret;
}
int rtw_parse_wpa2_ie(u8* rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher)
{
int i, ret=_SUCCESS;
int left, count;
u8 *pos;
if (rsn_ie_len <= 0) {
/* No RSN IE - fail silently */
return _FAIL;
}
if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (u8)(rsn_ie_len - 2)))
{
return _FAIL;
}
pos = rsn_ie;
pos += 4;
left = rsn_ie_len - 4;
//group_cipher
if (left >= RSN_SELECTOR_LEN) {
*group_cipher = rtw_get_wpa2_cipher_suite(pos);
pos += RSN_SELECTOR_LEN;
left -= RSN_SELECTOR_LEN;
} else if (left > 0) {
RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie length mismatch, %u too much", __FUNCTION__, left));
return _FAIL;
}
//pairwise_cipher
if (left >= 2)
{
//count = le16_to_cpu(*(u16*)pos);
count = RTW_GET_LE16(pos);
pos += 2;
left -= 2;
if (count == 0 || left < count * RSN_SELECTOR_LEN) {
RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie count botch (pairwise), "
"count %u left %u", __FUNCTION__, count, left));
return _FAIL;
}
for (i = 0; i < count; i++)
{
*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
pos += RSN_SELECTOR_LEN;
left -= RSN_SELECTOR_LEN;
}
}
else if (left == 1)
{
RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("%s: ie too short (for key mgmt)", __FUNCTION__));
return _FAIL;
}
return ret;
}
int rtw_parse_wpa2_ie(uint8_t * rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
{
int i, ret=_SUCCESS;
int left, count;
uint8_t *pos;
uint8_t SUITE_1X[4] = {0x00,0x0f, 0xac, 0x01};
if (rsn_ie_len <= 0) {
/* No RSN IE - fail silently */
return _FAIL;
}
if ((*rsn_ie!= _WPA2_IE_ID_) || (*(rsn_ie+1) != (uint8_t)(rsn_ie_len - 2)))
{
return _FAIL;
}
pos = rsn_ie;
pos += 4;
left = rsn_ie_len - 4;
//group_cipher
if (left >= RSN_SELECTOR_LEN) {
*group_cipher = rtw_get_wpa2_cipher_suite(pos);
pos += RSN_SELECTOR_LEN;
left -= RSN_SELECTOR_LEN;
} else if (left > 0) {
return _FAIL;
}
//pairwise_cipher
if (left >= 2)
{
//count = le16_to_cpu(*(u16 *)pos);
count = RTW_GET_LE16(pos);
pos += 2;
left -= 2;
if (count == 0 || left < count * RSN_SELECTOR_LEN) {
return _FAIL;
}
for (i = 0; i < count; i++)
{
*pairwise_cipher |= rtw_get_wpa2_cipher_suite(pos);
pos += RSN_SELECTOR_LEN;
left -= RSN_SELECTOR_LEN;
}
}
else if (left == 1)
{
return _FAIL;
}
if (is_8021x) {
if (left >= 6) {
pos += 2;
if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
*is_8021x = 1;
}
}
}
return ret;
}
int rtw_parse_wpa_ie(uint8_t * wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x)
{
int i, ret=_SUCCESS;
int left, count;
uint8_t *pos;
uint8_t SUITE_1X[4] = {0x00, 0x50, 0xf2, 1};
if (wpa_ie_len <= 0) {
/* No WPA IE - fail silently */
return _FAIL;
}
if ((*wpa_ie != _WPA_IE_ID_) || (*(wpa_ie+1) != (uint8_t)(wpa_ie_len - 2)) ||
(_rtw_memcmp(wpa_ie+2, RTW_WPA_OUI_TYPE, WPA_SELECTOR_LEN) != true) )
{
return _FAIL;
}
pos = wpa_ie;
pos += 8;
left = wpa_ie_len - 8;
//group_cipher
if (left >= WPA_SELECTOR_LEN) {
*group_cipher = rtw_get_wpa_cipher_suite(pos);
pos += WPA_SELECTOR_LEN;
left -= WPA_SELECTOR_LEN;
}
else if (left > 0)
{
return _FAIL;
}
//pairwise_cipher
if (left >= 2)
{
//count = le16_to_cpu(*(u16 *)pos);
count = RTW_GET_LE16(pos);
pos += 2;
left -= 2;
if (count == 0 || left < count * WPA_SELECTOR_LEN) {
return _FAIL;
}
for (i = 0; i < count; i++)
{
*pairwise_cipher |= rtw_get_wpa_cipher_suite(pos);
pos += WPA_SELECTOR_LEN;
left -= WPA_SELECTOR_LEN;
}
}
else if (left == 1)
{
return _FAIL;
}
if (is_8021x) {
if (left >= 6) {
pos += 2;
if (_rtw_memcmp(pos, SUITE_1X, 4) == 1) {
*is_8021x = 1;
}
}
}
return ret;
}
