int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value)
{
IOL_CMD cmd = {0x0, IOL_CMD_WW_REG, 0x0, 0x0};
RTW_PUT_BE16((u8*)&cmd.address, (u16)addr);
RTW_PUT_BE32((u8*)&cmd.value, (u32)value);
return rtw_IOL_append_cmds(xmit_frame, (u8*)&cmd, 8);
}
u32 build_prov_disc_request_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf, u8* pssid, u8 ussidlen, u8* pdev_raddr )
{
u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
u32 len=0, p2pielen = 0;
// P2P OUI
p2pielen = 0;
p2pie[ p2pielen++ ] = 0x50;
p2pie[ p2pielen++ ] = 0x6F;
p2pie[ p2pielen++ ] = 0x9A;
p2pie[ p2pielen++ ] = 0x09; // WFA P2P v1.0
// Commented by Albert 20110301
// According to the P2P Specification, the provision discovery request frame should contain 3 P2P attributes
// 1. P2P Capability
// 2. Device Info
// 3. Group ID ( When joining an operating P2P Group )
// P2P Capability ATTR
// Type:
p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
// Length:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
p2pielen += 2;
// Value:
// Device Capability Bitmap, 1 byte
// Be able to participate in additional P2P Groups and
// support the P2P Invitation Procedure
p2pie[ p2pielen++ ] = P2P_DEVCAP_INVITATION_PROC;
// Group Capability Bitmap, 1 byte
p2pie[ p2pielen++ ] = 0x00;
// Device Info ATTR
// Type:
p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
// Length:
// 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
// + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
p2pielen += 2;
// Value:
// P2P Device Address
_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );
p2pielen += ETH_ALEN;
// Config Method
// This field should be big endian. Noted by P2P specification.
if ( pwdinfo->ui_got_wps_info == P2P_GOT_WPSINFO_PBC )
{
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_PBC );
RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_PBC);
}
else
{
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_CONFIG_METHOD_DISPLAY );
RTW_PUT_BE16(p2pie + p2pielen, WPS_CONFIG_METHOD_DISPLAY);
}
p2pielen += 2;
// Primary Device Type
// Category ID
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_RTK_WIDI);
p2pielen += 2;
// OUI
//*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
p2pielen += 4;
// Sub Category ID
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_RTK_DMP);
p2pielen += 2;
// Number of Secondary Device Types
p2pie[ p2pielen++ ] = 0x00; // No Secondary Device Type List
// Device Name
// Type:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
p2pielen += 2;
// Length:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
p2pielen += 2;
// Value:
_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
p2pielen += pwdinfo->device_name_len;
if ( pwdinfo->role == P2P_ROLE_CLIENT )
{
// Added by Albert 2011/05/19
// In this case, the pdev_raddr is the device address of the group owner.
// P2P Group ID ATTR
// Type:
p2pie[ p2pielen++ ] = P2P_ATTR_GROUP_ID;
// Length:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( ETH_ALEN + ussidlen );
RTW_PUT_LE16(p2pie + p2pielen, ETH_ALEN + ussidlen);
p2pielen += 2;
// Value:
_rtw_memcpy( p2pie + p2pielen, pdev_raddr, ETH_ALEN );
p2pielen += ETH_ALEN;
_rtw_memcpy( p2pie + p2pielen, pssid, ussidlen );
p2pielen += ussidlen;
}
pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
return len;
}
u32 build_probe_resp_p2p_ie(struct wifidirect_info *pwdinfo, u8 *pbuf)
{
u8 p2pie[ MAX_P2P_IE_LEN] = { 0x00 };
u32 len=0, p2pielen = 0;
// P2P OUI
p2pielen = 0;
p2pie[ p2pielen++ ] = 0x50;
p2pie[ p2pielen++ ] = 0x6F;
p2pie[ p2pielen++ ] = 0x9A;
p2pie[ p2pielen++ ] = 0x09; // WFA P2P v1.0
// Commented by Albert 20100907
// According to the P2P Specification, the probe response frame should contain 5 P2P attributes
// 1. P2P Capability
// 2. Extended Listen Timing
// 3. Notice of Absence ( NOA ) ( Only GO needs this )
// 4. Device Info
// 5. Group Info ( Only GO need this )
// P2P Capability ATTR
// Type:
p2pie[ p2pielen++ ] = P2P_ATTR_CAPABILITY;
// Length:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0002 );
RTW_PUT_LE16(p2pie + p2pielen, 0x0002);
p2pielen += 2;
// Value:
// Device Capability Bitmap, 1 byte
// Be able to participate in additional P2P Groups and
// support the P2P Invitation Procedure
p2pie[ p2pielen++ ] = P2P_DEVCAP_INVITATION_PROC|P2P_DEVCAP_CLIENT_DISCOVERABILITY;
// Group Capability Bitmap, 1 byte
if(pwdinfo->role == P2P_ROLE_GO)
{
p2pie[ p2pielen ] = (P2P_GRPCAP_GO | P2P_GRPCAP_INTRABSS);
if(pwdinfo->p2p_state == P2P_STATE_PROVISIONING_ING)
p2pie[ p2pielen ] |= P2P_GRPCAP_GROUP_FORMATION;
p2pielen++;
}
else if ( pwdinfo->role == P2P_ROLE_DEVICE )
{
// Group Capability Bitmap, 1 byte
p2pie[ p2pielen++ ] = 0x00;
}
// Extended Listen Timing ATTR
// Type:
p2pie[ p2pielen++ ] = P2P_ATTR_EX_LISTEN_TIMING;
// Length:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0x0004 );
RTW_PUT_LE16(p2pie + p2pielen, 0x0004);
p2pielen += 2;
// Value:
// Availability Period
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
p2pielen += 2;
// Availability Interval
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 0xFFFF );
RTW_PUT_LE16(p2pie + p2pielen, 0xFFFF);
p2pielen += 2;
// Notice of Absence ATTR
// Type:
// Length:
// Value:
if(pwdinfo->role == P2P_ROLE_GO)
{
//go_add_noa_attr(pwdinfo);
}
// Device Info ATTR
// Type:
p2pie[ p2pielen++ ] = P2P_ATTR_DEVICE_INFO;
// Length:
// 21 -> P2P Device Address (6bytes) + Config Methods (2bytes) + Primary Device Type (8bytes)
// + NumofSecondDevType (1byte) + WPS Device Name ID field (2bytes) + WPS Device Name Len field (2bytes)
//*(u16*) ( p2pie + p2pielen ) = cpu_to_le16( 21 + pwdinfo->device_name_len );
RTW_PUT_LE16(p2pie + p2pielen, 21 + pwdinfo->device_name_len);
p2pielen += 2;
// Value:
// P2P Device Address
_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_addr, ETH_ALEN );
p2pielen += ETH_ALEN;
// Config Method
// This field should be big endian. Noted by P2P specification.
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->supported_wps_cm );
RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->supported_wps_cm);
p2pielen += 2;
// Primary Device Type
// Category ID
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_CID_RTK_WIDI );
RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_CID_RTK_WIDI);
p2pielen += 2;
// OUI
//*(u32*) ( p2pie + p2pielen ) = cpu_to_be32( WPSOUI );
RTW_PUT_BE32(p2pie + p2pielen, WPSOUI);
p2pielen += 4;
// Sub Category ID
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_PDT_SCID_RTK_DMP );
RTW_PUT_BE16(p2pie + p2pielen, WPS_PDT_SCID_RTK_DMP);
p2pielen += 2;
// Number of Secondary Device Types
p2pie[ p2pielen++ ] = 0x00; // No Secondary Device Type List
// Device Name
// Type:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
RTW_PUT_BE16(p2pie + p2pielen, WPS_ATTR_DEVICE_NAME);
p2pielen += 2;
// Length:
//*(u16*) ( p2pie + p2pielen ) = cpu_to_be16( pwdinfo->device_name_len );
RTW_PUT_BE16(p2pie + p2pielen, pwdinfo->device_name_len);
p2pielen += 2;
// Value:
_rtw_memcpy( p2pie + p2pielen, pwdinfo->device_name, pwdinfo->device_name_len );
p2pielen += pwdinfo->device_name_len;
// Group Info ATTR
// Type:
// Length:
// Value:
if(pwdinfo->role == P2P_ROLE_GO)
{
p2pielen += go_add_group_info_attr(pwdinfo, p2pie + p2pielen);
}
pbuf = rtw_set_ie(pbuf, _VENDOR_SPECIFIC_IE_, p2pielen, (unsigned char *) p2pie, &len);
return len;
}
static u32 go_add_group_info_attr(struct wifidirect_info *pwdinfo, u8 *pbuf)
{
_list *phead, *plist;
u32 len=0;
u16 attr_len = 0;
u8 tmplen, *pdata_attr, *pstart, *pcur;
struct sta_info *psta = NULL;
_adapter *padapter = pwdinfo->padapter;
struct sta_priv *pstapriv = &padapter->stapriv;
DBG_871X("%s\n", __FUNCTION__);
pdata_attr = rtw_zmalloc(MAX_P2P_IE_LEN);
pstart = pdata_attr;
pcur = pdata_attr;
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)
{
tmplen = 0;
pcur++;
//P2P device address
_rtw_memcpy(pcur, psta->dev_addr, ETH_ALEN);
pcur += ETH_ALEN;
//P2P interface address
_rtw_memcpy(pcur, psta->hwaddr, ETH_ALEN);
pcur += ETH_ALEN;
*pcur = psta->dev_cap;
pcur++;
//*(u16*)(pcur) = cpu_to_be16(psta->config_methods);
RTW_PUT_BE16(pcur, psta->config_methods);
pcur += 2;
_rtw_memcpy(pcur, psta->primary_dev_type, 8);
pcur += 8;
*pcur = psta->num_of_secdev_type;
pcur++;
_rtw_memcpy(pcur, psta->secdev_types_list, psta->num_of_secdev_type*8);
pcur += psta->num_of_secdev_type*8;
if(psta->dev_name_len>0)
{
//*(u16*)(pcur) = cpu_to_be16( WPS_ATTR_DEVICE_NAME );
RTW_PUT_BE16(pcur, WPS_ATTR_DEVICE_NAME);
pcur += 2;
//*(u16*)(pcur) = cpu_to_be16( psta->dev_name_len );
RTW_PUT_BE16(pcur, psta->dev_name_len);
pcur += 2;
_rtw_memcpy(pcur, psta->dev_name, psta->dev_name_len);
pcur += psta->dev_name_len;
}
tmplen = (u8)(pcur-pstart);
*pstart = (tmplen-1);
attr_len += tmplen;
//pstart += tmplen;
pstart = pcur;
}
}
if(attr_len>0)
{
len = rtw_set_p2p_attr_content(pbuf, P2P_ATTR_GROUP_INFO, attr_len, pdata_attr);
}
rtw_mfree(pdata_attr, MAX_P2P_IE_LEN);
return len;
}
static void issue_p2p_provision_resp(struct wifidirect_info *pwdinfo, u8* raddr, u8* frame_body, u16 config_method)
{
_adapter *padapter = pwdinfo->padapter;
unsigned char category = WLAN_CATEGORY_PUBLIC;
u8 action = P2P_PUB_ACTION_ACTION;
u8 dialogToken = frame_body[7]; // The Dialog Token of provisioning discovery request frame.
u32 p2poui = cpu_to_be32(P2POUI);
u8 oui_subtype = P2P_PROVISION_DISC_RESP;
u8 wpsie[ 100 ] = { 0x00 };
u8 wpsielen = 0;
struct xmit_frame *pmgntframe;
struct pkt_attrib *pattrib;
unsigned char *pframe;
struct ieee80211_hdr *pwlanhdr;
unsigned short *fctrl;
struct xmit_priv *pxmitpriv = &(padapter->xmitpriv);
struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
{
return;
}
//update attribute
pattrib = &pmgntframe->attrib;
update_mgntframe_attrib(padapter, pattrib);
_rtw_memset(pmgntframe->buf_addr, 0, WLANHDR_OFFSET + TXDESC_OFFSET);
pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
pwlanhdr = (struct ieee80211_hdr *)pframe;
fctrl = &(pwlanhdr->frame_ctl);
*(fctrl) = 0;
_rtw_memcpy(pwlanhdr->addr1, raddr, ETH_ALEN);
_rtw_memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
_rtw_memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
SetSeqNum(pwlanhdr, pmlmeext->mgnt_seq);
pmlmeext->mgnt_seq++;
SetFrameSubType(pframe, WIFI_ACTION);
pframe += sizeof(struct ieee80211_hdr_3addr);
pattrib->pktlen = sizeof(struct ieee80211_hdr_3addr);
pframe = rtw_set_fixed_ie(pframe, 1, &(category), &(pattrib->pktlen));
pframe = rtw_set_fixed_ie(pframe, 1, &(action), &(pattrib->pktlen));
pframe = rtw_set_fixed_ie(pframe, 4, (unsigned char *) &(p2poui), &(pattrib->pktlen));
pframe = rtw_set_fixed_ie(pframe, 1, &(oui_subtype), &(pattrib->pktlen));
pframe = rtw_set_fixed_ie(pframe, 1, &(dialogToken), &(pattrib->pktlen));
wpsielen = 0;
// WPS OUI
//*(u32*) ( wpsie ) = cpu_to_be32( WPSOUI );
RTW_PUT_BE32(wpsie, WPSOUI);
wpsielen += 4;
#if 0
// WPS version
// Type:
*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_VER1 );
wpsielen += 2;
// Length:
*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0001 );
wpsielen += 2;
// Value:
wpsie[wpsielen++] = WPS_VERSION_1; // Version 1.0
#endif
// Config Method
// Type:
//*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( WPS_ATTR_CONF_METHOD );
RTW_PUT_BE16(wpsie + wpsielen, WPS_ATTR_CONF_METHOD);
wpsielen += 2;
// Length:
//*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( 0x0002 );
RTW_PUT_BE16(wpsie + wpsielen, 0x0002);
wpsielen += 2;
// Value:
//*(u16*) ( wpsie + wpsielen ) = cpu_to_be16( config_method );
RTW_PUT_BE16(wpsie + wpsielen, config_method);
wpsielen += 2;
pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, wpsielen, (unsigned char *) wpsie, &pattrib->pktlen );
pattrib->last_txcmdsz = pattrib->pktlen;
dump_mgntframe(padapter, pmgntframe);
return;
}