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; }