/* *************************************************************************** * Ralink Tech Inc. * 4F, No. 2 Technology 5th Rd. * Science-based Industrial Park * Hsin-chu, Taiwan, R.O.C. * * (c) Copyright 2002-2010, Ralink Technology, Inc. * * All rights reserved. Ralink's source code is an unpublished work and the * use of a copyright notice does not imply otherwise. This source code * contains confidential trade secret material of Ralink Tech. Any attemp * or participation in deciphering, decoding, reverse engineering or in any * way altering the source code is stricitly prohibited, unless the prior * written consent of Ralink Technology, Inc. is obtained. *************************************************************************** Module Name: p2p_packet.c Abstract: Peer to peer is also called Wifi Direct. P2P is a Task Group of WFA. this file contains all functions that handles p2p packets Revision History: Who When What -------- ---------- ---------------------------------------------- Jan Lee 2010-03-08 created for Peer-to-Peer(Wifi Direct) */ #include "rt_config.h" extern UCHAR OutMsgBuf[]; /* buffer to create message contents */ extern UCHAR WILDP2PSSID[]; extern UCHAR WILDP2PSSIDLEN; extern UCHAR WIFIDIRECT_OUI[]; extern UCHAR P2POUIBYTE[]; extern UCHAR ZERO_MAC_ADDR[]; extern UCHAR WPS_OUI[]; extern UCHAR STA_Wsc_Pri_Dev_Type[]; extern UCHAR AP_Wsc_Pri_Dev_Type[]; #ifdef WFD_SUPPORT extern UCHAR WIFIDISPLAY_OUI[]; #endif /* WFD_SUPPORT */ extern INT Set_P2p_OpMode_Proc( IN PRTMP_ADAPTER pAd, IN PSTRING arg); extern INT Set_P2pCli_Enable_Proc( IN PRTMP_ADAPTER pAd, IN PSTRING arg); /* ========================================================================== Description: Publiac action frame. But with ACtion is GAS_INITIAL_REQ (11). 802.11u. 7.4.7.10 Parameters: Note: ========================================================================== */ VOID PeerGASIntialReqAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { UCHAR Action = Elem->Msg[LENGTH_802_11+1]; UCHAR p2pindex; UCHAR Addr2[6]; PFRAME_802_11 pFrame; PRT_P2P_CLIENT_ENTRY pP2pEntry; MLME_P2P_ACTION_STRUCT P2PActReq; UINT ServiceDiscQueryLen = 0; PUCHAR ServiceDiscQuery = NULL; pFrame = (PFRAME_802_11)Elem->Msg; DBGPRINT(RT_DEBUG_TRACE, ("P2pPeer GASIntialReqAction = %d, \n", Elem->MsgLen)); COPY_MAC_ADDR(Addr2, pFrame->Hdr.Addr2); DBGPRINT(RT_DEBUG_TRACE, ("From Addr2 = %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(Addr2))); #ifdef WFD_SUPPORT os_alloc_mem(pAd, &ServiceDiscQuery, MAX_VIE_LEN); if (ServiceDiscQuery == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s:: Allocate memory size(=1024) failed\n", __FUNCTION__)); return; } #endif /* WFD_SUPPORT */ p2pindex = P2pGroupTabSearch(pAd, Addr2); if (p2pindex == P2P_NOT_FOUND) { p2pindex = P2pGroupTabInsert(pAd, Addr2, P2PSTATE_DISCOVERY, NULL, 0, 0, 0); } if (p2pindex < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[p2pindex]; #ifdef WFD_SUPPORT RTMPZeroMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info, sizeof(WFD_SERV_DISC_QUERY_INFO)); #endif /* WFD_SUPPORT */ hex_dump("----> P2PParseServiceDiscoReq", Elem->Msg, Elem->MsgLen); P2PParseServiceDiscoReq(pAd, pFrame, &pP2pEntry->DialogToken, &pAd->P2pCfg.ServiceTransac, ServiceDiscQuery, &ServiceDiscQueryLen); #ifdef WFD_SUPPORT /* Query All WFD Service */ if (ServiceDiscQueryLen == 3) { pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_device_info_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_associate_bssid_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_audio_format_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_video_format_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_3d_video_format_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_content_proctection = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_couple_sink_info_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_extent_capability_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_local_ip_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_session_info_ie = TRUE; pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_alternate_mac_addr_ie = TRUE; } if ((ServiceDiscQueryLen > 3)) { PUCHAR pWfdEid = NULL; INT iLen; hex_dump("P2PParseServiceDiscoReq:: Query", ServiceDiscQuery, (ServiceDiscQueryLen - 3)); pWfdEid = ServiceDiscQuery; for (iLen = ServiceDiscQueryLen - 3; iLen > 0; iLen--) { switch (*pWfdEid) { case SUBID_WFD_DEVICE_INFO: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_device_info_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_DEVICE_INFO\n", __FUNCTION__)); break; case SUBID_WFD_ASSOCIATED_BSSID: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_associate_bssid_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_ASSOCIATED_BSSID\n", __FUNCTION__)); break; case SUBID_WFD_AUDIO_FORMATS: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_audio_format_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_AUDIO_FORMATS\n", __FUNCTION__)); break; case SUBID_WFD_VIDEO_FORMATS: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_video_format_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_VIDEO_FORMATS\n", __FUNCTION__)); break; case SUBID_WFD_3D_VIDEO_FORMATS: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_3d_video_format_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_3D_VIDEO_FORMATS\n", __FUNCTION__)); break; case SUBID_WFD_CONTENT_PROTECTION: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_content_proctection = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_CONTENT_PROTECTION\n", __FUNCTION__)); break; case SUBID_WFD_COUPLED_SINK_INFO: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_couple_sink_info_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_COUPLED_SINK_INFO\n", __FUNCTION__)); break; case SUBID_WFD_EXTENDED_CAP: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_extent_capability_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_EXTENDED_CAP\n", __FUNCTION__)); break; case SUBID_WFD_LOCAL_IP_ADDR: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_local_ip_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_LOCAL_IP_ADDR\n", __FUNCTION__)); break; case SUBID_WFD_SESSION_INFO: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_session_info_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_SESSION_INFO\n", __FUNCTION__)); break; case SUBID_WFD_ALTERNATE_MAC_ADDR: pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.bWfd_alternate_mac_addr_ie = TRUE; DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_ALTERNATE_MAC_ADDR\n", __FUNCTION__)); break; default: DBGPRINT(RT_DEBUG_ERROR, (" SUBID_WFD_ unknown Eid = %x \n", *pWfdEid)); //hex_dump("WfdParseSubElement::", Msg, MsgLen); break; } pWfdEid++; } } os_free_mem(pAd, ServiceDiscQuery); #endif /* WFD_SUPPORT */ NdisZeroMemory(&P2PActReq, sizeof(P2PActReq)); COPY_MAC_ADDR(P2PActReq.Addr, pP2pEntry->addr); DBGPRINT(RT_DEBUG_TRACE, (" = %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(P2PActReq.Addr))); P2PActReq.TabIndex = p2pindex; MlmeEnqueue(pAd, P2P_ACTION_STATE_MACHINE, MT2_MLME_P2P_GAS_INT_RSP, sizeof(MLME_P2P_ACTION_STRUCT), (PVOID)&P2PActReq, 0); MlmeHandler(pAd); } } /* ========================================================================== Description: Publiac action frame. But with ACtion is GAS_INITIAL_RSP (12). 802.11u. 7.4.7.11 Parameters: Note: ========================================================================== */ VOID PeerGASIntialRspAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { UCHAR Action = Elem->Msg[LENGTH_802_11+1]; UCHAR p2pindex; UCHAR Addr2[6]; UCHAR StatusCode; USHORT ComeBackDelay; PUCHAR pAdProtocolElem; PUCHAR pQueryRsp; ULONG i; PUCHAR pDest; PRT_P2P_CLIENT_ENTRY pP2pEntry; PFRAME_802_11 pFrame; UINT ServiceDiscQueryLen = 0; PUCHAR ServiceDiscQuery = NULL; pFrame = (PFRAME_802_11)Elem->Msg; DBGPRINT(RT_DEBUG_TRACE, ("P2pPeer PeerGASIntialRspAction = %d.\n", Elem->MsgLen)); #ifdef WFD_SUPPORT os_alloc_mem(pAd, &ServiceDiscQuery, MAX_VIE_LEN); if (ServiceDiscQuery == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s:: Allocate memory size(=1024) failed\n", __FUNCTION__)); return; } #endif /* WFD_SUPPORT */ COPY_MAC_ADDR(Addr2, pFrame->Hdr.Addr2); DBGPRINT(RT_DEBUG_TRACE, ("From Addr2 = %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(Addr2))); pDest = &Elem->Msg[0]; hex_dump("PeerGASIntialRspAction", Elem->Msg, Elem->MsgLen); p2pindex = P2pGroupTabSearch(pAd, Addr2); if (p2pindex == P2P_NOT_FOUND) { p2pindex = P2pGroupTabInsert(pAd, Addr2, P2PSTATE_DISCOVERY, NULL, 0, 0, 0); } if (p2pindex < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[p2pindex]; #ifdef WFD_SUPPORT RTMPZeroMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info, sizeof(WFD_SERV_DISC_QUERY_INFO)); #endif /* WFD_SUPPORT */ hex_dump("----> P2PParseServiceDiscoReq", Elem->Msg, Elem->MsgLen); P2PParseServiceDiscoRsp(pAd, pP2pEntry, p2pindex, pFrame, &pAd->P2pCfg.ServiceTransac, ServiceDiscQuery, &ServiceDiscQueryLen); #ifdef WFD_SUPPORT if ((ServiceDiscQueryLen > 3)) { PP2PEID_STRUCT pWfdEid; PEID_STRUCT pEid; PUCHAR pWfdIe = NULL; ULONG AccuWfdIELen = 0, AccuIeLen = 0, Length = 0, AttriLen = 0; hex_dump("P2PParseServiceDiscoRsp:: Query", ServiceDiscQuery, (ServiceDiscQueryLen - 3)); pWfdIe = pWfdEid = (PP2PEID_STRUCT) ServiceDiscQuery; printk("WfdEid = %d. WfdEid_Len = %02x %02x.\n", pWfdEid->Eid, pWfdEid->Len[0], pWfdEid->Len[1]); AttriLen = pWfdEid->Len[1] + (pWfdEid->Len[0] << 8); printk("AttriLen = %d. SerDiscQueryLen = %d.\n", AttriLen, ServiceDiscQueryLen); while (AttriLen <= (ServiceDiscQueryLen - 2)) { AttriLen = pWfdEid->Len[1] + (pWfdEid->Len[0] << 8); printk(" AttriLen = %d.\n", AttriLen); Length = 0; printk("Eid = %d. Length = %d.\n", pWfdEid->Eid, Length); switch (pWfdEid->Eid) { case SUBID_WFD_DEVICE_INFO: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_DEVICE_INFO(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_DEVICE_INFO", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_device_info_ie, pWfdEid->Octet, SUBID_WFD_DEVICE_INFO_LEN); break; case SUBID_WFD_ASSOCIATED_BSSID: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_ASSOCIATED_BSSID(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_ASSOCIATED_BSSID", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_associate_bssid_ie, pWfdEid->Octet, SUBID_WFD_ASSOCIATED_BSSID_LEN); break; case SUBID_WFD_AUDIO_FORMATS: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_AUDIO_FORMATS(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_AUDIO_FORMATS", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_audio_format_ie, pWfdEid->Octet, SUBID_WFD_AUDIO_FORMATS_LEN); break; case SUBID_WFD_VIDEO_FORMATS: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_VIDEO_FORMATS(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_VIDEO_FORMATS", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_video_format_ie, pWfdEid->Octet, SUBID_WFD_VIDEO_FORMATS_LEN); break; case SUBID_WFD_3D_VIDEO_FORMATS: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_3D_VIDEO_FORMATS(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_3D_VIDEO_FORMATS", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_3d_video_format_ie, pWfdEid->Octet, SUBID_WFD_3D_VIDEO_FORMATS_LEN); break; case SUBID_WFD_CONTENT_PROTECTION: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_CONTENT_PROTECTION(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_CONTENT_PROTECTION", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_content_proctection, pWfdEid->Octet, SUBID_WFD_CONTENT_PROTECTION_LEN); break; case SUBID_WFD_COUPLED_SINK_INFO: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_COUPLED_SINK_INFO(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_COUPLED_SINK_INFO", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_couple_sink_info_ie, pWfdEid->Octet, SUBID_WFD_COUPLED_SINK_INFO_LEN); break; case SUBID_WFD_EXTENDED_CAP: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_EXTENDED_CAP(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_EXTENDED_CAP", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_extent_capability_ie, pWfdEid->Octet, SUBID_WFD_EXTENDED_CAP_LEN); break; case SUBID_WFD_LOCAL_IP_ADDR: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_LOCAL_IP_ADDR(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_LOCAL_IP_ADDR", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_local_ip_ie, pWfdEid->Octet, SUBID_WFD_LOCAL_IP_ADDR_LEN); break; case SUBID_WFD_SESSION_INFO: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_SESSION_INFO(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_SESSION_INFO", pWfdEid->Octet, AttriLen); break; case SUBID_WFD_ALTERNATE_MAC_ADDR: DBGPRINT(RT_DEBUG_TRACE, ("%s::SUBID_WFD_ALTERNATE_MAC_ADDR(%d)\n", __FUNCTION__, AttriLen)); hex_dump("SUBID_WFD_ALTERNATE_MAC_ADDR", pWfdEid->Octet, AttriLen); RTMPMoveMemory(&pP2pEntry->WfdEntryInfo.wfd_serv_disc_query_info.wfd_alternate_mac_addr_ie, pWfdEid->Octet, SUBID_WFD_ALTERNATE_MAC_ADDR_LEN); break; default: DBGPRINT(RT_DEBUG_ERROR, (" SUBID_WFD_ unknown Eid = %x \n", pWfdEid->Eid)); //hex_dump("WfdParseSubElement::", Msg, MsgLen); break; } Length = Length + 3 + AttriLen; /* Eid[1] + Len[2] + content[Len] */ if (Length >= ServiceDiscQueryLen - 2) break; pWfdEid = (PP2PEID_STRUCT)((UCHAR*)pWfdEid + 3 + AttriLen); AttriLen = pWfdEid->Len[1] + (pWfdEid->Len[0] << 8); } } os_free_mem(pAd, ServiceDiscQuery); #endif /* WFD_SUPPORT */ } } /* ========================================================================== Description: Publiac action frame. But with ACtion is GAS_COMEBACK_REQ (11). 802.11u. 7.4.7.10 Parameters: Note: ========================================================================== */ VOID PeerGASComebackReqAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { UCHAR Action = Elem->Msg[LENGTH_802_11+1]; UCHAR p2pindex; UCHAR Addr2[6]; ULONG i; PUCHAR pDest; PRT_P2P_CLIENT_ENTRY pP2pEntry; PFRAME_802_11 pFrame; MLME_P2P_ACTION_STRUCT P2PActReq; pFrame = (PFRAME_802_11)Elem->Msg; DBGPRINT(RT_DEBUG_TRACE,("p2p PeerGASComebackReqAction = %d, \n", Elem->MsgLen)); COPY_MAC_ADDR(Addr2, pFrame->Hdr.Addr2); DBGPRINT(RT_DEBUG_TRACE, ("From Addr2 = %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(Addr2))); p2pindex = P2pGroupTabSearch(pAd, Addr2); if (p2pindex == P2P_NOT_FOUND) { p2pindex = P2pGroupTabInsert(pAd, Addr2, P2PSTATE_DISCOVERY, NULL, 0, 0, 0); } if (p2pindex < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[p2pindex]; if (TRUE == P2PParseComebackReq(pAd, pFrame, &pP2pEntry->DialogToken)) { NdisZeroMemory(&P2PActReq, sizeof(P2PActReq)); COPY_MAC_ADDR(P2PActReq.Addr, pP2pEntry->addr); DBGPRINT(RT_DEBUG_TRACE, (" = %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(P2PActReq.Addr))); P2PActReq.TabIndex = p2pindex; MlmeEnqueue(pAd, P2P_ACTION_STATE_MACHINE, MT2_MLME_P2P_GAS_CB_RSP, sizeof(MLME_P2P_ACTION_STRUCT), (PVOID)&P2PActReq, 0); MlmeHandler(pAd); } } } /* ========================================================================== Description: Publiac action frame. But with ACtion is GAS_COMEBACK_Rsp(11). 802.11u. 7.4.7.10 Parameters: Note: ========================================================================== */ VOID PeerGASComebackRspAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PFRAME_802_11 pFrame; UCHAR p2pindex; UCHAR Addr2[6]; PRT_P2P_CLIENT_ENTRY pP2pEntry; pFrame = (PFRAME_802_11)Elem->Msg; COPY_MAC_ADDR(Addr2, pFrame->Hdr.Addr2); p2pindex = P2pGroupTabSearch(pAd, Addr2); DBGPRINT(RT_DEBUG_TRACE,("p2p PeerGASComebackRspAction = %d, \n", Elem->MsgLen)); if (p2pindex < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[p2pindex]; if (TRUE == P2PParseComebackRsp(pAd, pP2pEntry, pFrame, &pAd->P2pCfg.ServiceTransac)) pAd->P2pCfg.p2pidxForServiceCbReq = p2pindex; } } /* ========================================================================== Description: Parse P2P Service Discovery Request. Parameters: return : TRUE if .... We don't support Service discovery yet. So no rule now. Always TRUE first. Note: ========================================================================== */ BOOLEAN P2PParseServiceDiscoReq( IN PRTMP_ADAPTER pAd, IN PFRAME_802_11 pFrame, OUT UCHAR *DialogToken, OUT UCHAR *ServiceTransaction, OUT PUCHAR pServiceDiscQuery, OUT UINT *ServiceDiscQueryLen) { PUCHAR pData; LONG RemainQueryLen; INT i; pData = pFrame->Octet; do { if (*pData != CATEGORY_PUBLIC) { DBGPRINT(RT_DEBUG_ERROR,("GASIntialReq = Not Public action. Return \n")); return FALSE; } if (*(pData+1) != ACTION_GAS_INITIAL_REQ) { DBGPRINT(RT_DEBUG_ERROR,("GASIntialReq = Not GAS Initial Req. Return \n")); return FALSE; } /* The Dialog Token field is copied from the corresponding GAS Initial Request frame */ *DialogToken = *(pData + 2); pData += 3; if (*pData != 0x6c) { DBGPRINT(RT_DEBUG_ERROR,("GASIntialReq = wrong Advertisement protocol information element Return \n")); return FALSE; } pData += 4; pData += 2; if (*pData != 0xdd) { DBGPRINT(RT_DEBUG_ERROR,("GASIntialReq = wrong Return \n")); return FALSE; } RemainQueryLen = cpu2le16(*((PUSHORT)pData + 2)) - 6; if (!RTMPEqualMemory((pData + 4), P2POUIBYTE, 4)) { DBGPRINT(RT_DEBUG_ERROR,("GASIntialReq = wrong OUI Return \n")); return FALSE; } #ifdef WFD_SUPPORT pData += 10; for (i = 0; i < WFD_DEVICE_TYPE_END; i++) { if (RemainQueryLen > 0) { *ServiceDiscQueryLen = cpu2le16(*((PUSHORT)pData)); if (*ServiceDiscQueryLen > 3) { DBGPRINT(RT_DEBUG_TRACE, ("Query Service Discovery Len = %d. QueryType = %d.\n", *ServiceDiscQueryLen, *(pData + 2))); *ServiceTransaction = *(pData + 3); DBGPRINT(RT_DEBUG_TRACE,("ServiceTransac is %d. \n", *ServiceTransaction)); DBGPRINT(RT_DEBUG_TRACE,("Requested Device Role is 0x%02x. \n", *(pData + 4))); if (pAd->StaCfg.WfdCfg.DeviceType == *(pData + 4)) { DBGPRINT(RT_DEBUG_TRACE,("My Device Type is 0x%02x. \n", *(pData + 4))); if ((*ServiceDiscQueryLen - 3)) RTMPMoveMemory(pServiceDiscQuery, (pData + 5), (*ServiceDiscQueryLen - 3)); /* Query Len = Query Type(1) + TransactionID(1) + Requested Device Role(1) + QueryData(?) */ break; } } pData += *ServiceDiscQueryLen + 2; RemainQueryLen -= *ServiceDiscQueryLen + 2; } else break; } #endif /* WFD_SUPPORT */ }while(FALSE); return TRUE; } /* ========================================================================== Description: Parse P2P Service Discovery Response. Parameters: return : TRUE if .... We don't support Service discovery yet. So no rule now. Always TRUE first. Note: ========================================================================== */ BOOLEAN P2PParseServiceDiscoRsp( IN PRTMP_ADAPTER pAd, IN PRT_P2P_CLIENT_ENTRY pP2pEntry, IN UCHAR pP2pidx, IN PFRAME_802_11 pFrame, OUT UCHAR *ServiceTransaction, OUT PUCHAR pServiceDiscQuery, OUT UINT *ServiceDiscQueryLen) { PUCHAR pData; pData = pFrame->Octet; do { if (*pData != CATEGORY_PUBLIC) { DBGPRINT(RT_DEBUG_ERROR,("P2PParseServiceDiscoRsp = Not Public action. Return \n")); return FALSE; } if (*(pData+1) != ACTION_GAS_INITIAL_RSP) { DBGPRINT(RT_DEBUG_ERROR,("P2PParseServiceDiscoRsp = Not GAS Initial RSP. Return \n")); return FALSE; } if (*(PUSHORT)(pData+3) != 0) { DBGPRINT(RT_DEBUG_ERROR,("P2PParseServiceDiscoRsp = Status is 0x%x. Return \n", *(PUSHORT)(pData+3))); return FALSE; } if (*(PUSHORT)(pData+5) != 0) { } pData += 7; if (*pData != 0x6c) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseServiceDiscoRsp = wrong Advertisement protocol information element Return \n")); return FALSE; } pData += 4; pData += 2; if (*pData != 0xdd) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseServiceDiscoRsp = wrong Return \n")); return FALSE; } if (!RTMPEqualMemory((pData + 4), P2POUIBYTE, 4)) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseServiceDiscoRsp = wrong OUI Return \n")); return FALSE; } *ServiceDiscQueryLen = cpu2le16(*((PUSHORT)(pData + 10))); DBGPRINT(RT_DEBUG_ERROR, ("Query Service Discovery Len = %d. QueryType = %d.\n", *ServiceDiscQueryLen, *(pData + 12))); *ServiceTransaction = *(pData+13); DBGPRINT(RT_DEBUG_ERROR,("ServiceTransac = %d. \n", *ServiceTransaction)); if ((*ServiceDiscQueryLen - 3)) RTMPMoveMemory(pServiceDiscQuery, (pData + 15), (*ServiceDiscQueryLen - 3)); /* Query Len = Query Type(1) + TransactionID(1) + StatusCode(1) + QueryData(?) */ }while(FALSE); return TRUE; } /* ========================================================================== Description: Parse P2P Service Comeback Request. Parameters: return : TRUE if .... We don't support Service discovery yet. So no rule now. Always TRUE first. Note: ========================================================================== */ BOOLEAN P2PParseComebackReq( IN PRTMP_ADAPTER pAd, IN PFRAME_802_11 pFrame, OUT UCHAR *DialogToken) { PUCHAR pData; pData = pFrame->Octet; do { if (*pData != CATEGORY_PUBLIC) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseComebackReq = Not Public action. Return \n")); return FALSE; } if (*(pData+1) != ACTION_GAS_COMEBACK_REQ) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseComebackReq = Not GAS_COMEBACK_REQ Return \n")); return FALSE; } /* The Dialog Token field is copied from the corresponding GAS Initial Request frame */ *DialogToken = *(pData + 2); }while(FALSE); return TRUE; } /* ========================================================================== Description: Parse P2P Service Comeback Response. Parameters: return : TRUE if .... We don't support Service discovery yet. So no rule now. Always TRUE first. Note: ========================================================================== */ BOOLEAN P2PParseComebackRsp( IN PRTMP_ADAPTER pAd, IN PRT_P2P_CLIENT_ENTRY pP2pEntry, IN PFRAME_802_11 pFrame, IN UCHAR *ServiceTransaction) { PUCHAR pData; pData = pFrame->Octet; do { if (*pData != CATEGORY_PUBLIC) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseComebackRsp = Not Public action. Return \n")); return FALSE; } if (*(pData+1) != ACTION_GAS_COMEBACK_RSP) { DBGPRINT(RT_DEBUG_TRACE,("P2PParseComebackRsp = Not GAS_COMEBACK_REQ Return \n")); return FALSE; } // +2 : Token // +3, +4 : Status // +5 : More Frag DBGPRINT(RT_DEBUG_TRACE,("P2PParseComebackRsp = More Frag = %d. \n", *(pData+5))); if ((*(pData+5) &0x80) == 0x80 ) { pP2pEntry->P2pClientState = P2PSTATE_SERVICE_COMEBACK_COMMAND; DBGPRINT(RT_DEBUG_TRACE,("P2PParseComebackRsp = More Frag set to one. \n")); return TRUE; } // +6 +7 : Comeback Delay // +8 : ADvertisement Potocol IE // +9 +10 : Query Response Len return FALSE; }while(FALSE); } /* ========================================================================== Description: Receive Public Action frame that set to verdor specific and with OUI type = WFA P2P ========================================================================== */ VOID P2PPublicAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { DBGPRINT(RT_DEBUG_ERROR, ("!!!!! Should not in here !!!!!\n")); } /* ========================================================================== Description: Make a P2P Fake NoA Attribute to trigger myself to restart NoA. The Start time is changed. Duration and Interval and Count is the same as GO's beacon Parameters: StartTime : A new Start time. pOutBuffer : pointer to buffer that should put data to. Note: ========================================================================== */ VOID P2PMakeFakeNoATlv( IN PRTMP_ADAPTER pAd, IN ULONG StartTime, IN PUCHAR pOutBuffer) { PUCHAR pDest; pDest = pOutBuffer; *(pDest) = SUBID_P2P_NOA; /* Length is 13*n + 2 = 15 when n = 1 */ *(pDest+1) = 15; /* Lenght 2nd byte */ *(pDest+2) = 0; /* Index. */ *(pDest+3) = pAd->P2pCfg.GONoASchedule.Token; /* CT Windows and OppPS parm. Don't turn on both. So Set CTWindows = 0 */ *(pDest+4) = 0; /* Count. Test Plan set to 255. */ *(pDest+5) = pAd->P2pCfg.GONoASchedule.Count; /* Duration */ RTMPMoveMemory((pDest+6), &pAd->P2pCfg.GONoASchedule.Duration, 4); /* Interval */ RTMPMoveMemory((pDest+10), &pAd->P2pCfg.GONoASchedule.Interval, 4); RTMPMoveMemory((pDest+14), &StartTime, 4); } /* ========================================================================== Description: Insert P2P subelement P2P Group Info format in Probe Response. it contains device information of P2P Clients that are members of my P2P group. Parameters: pInBuffer : pointer to data that contains data to put in pOutBuffer : pointer to buffer that should put data to. Note: ========================================================================== */ ULONG InsertP2PGroupInfoTlv( IN PRTMP_ADAPTER pAd, IN PUCHAR pOutBuffer) { PUCHAR pDest, pContent; UCHAR Length; UCHAR i; UCHAR NoOfClient = 0; P2P_CLIENT_INFO_DESC ClientInfo; RT_P2P_CLIENT_ENTRY *pClient; UCHAR ZeroType[P2P_DEVICE_TYPE_LEN]; UCHAR DevCapability; USHORT config_method = 0; pDest = pOutBuffer; RTMPZeroMemory(pDest, 255); RTMPZeroMemory(ZeroType, P2P_DEVICE_TYPE_LEN); *pDest = SUBID_P2P_GROUP_INFO; *(pDest + 2) = 0; /* Set length to 0 first. Need to update to real length in the end of this function. */ pContent = pDest + 3; /* pContent points to payload. */ Length = 0; for (i = 0;i < MAX_P2P_GROUP_SIZE;i++) { pClient = &pAd->P2pTable.Client[i]; /* if (IS_P2P_PEER_CLIENT_OP(pClient)) */ if (pClient->P2pClientState == P2PSTATE_CLIENT_WPS_DONE) { NoOfClient++; RTMPZeroMemory(&ClientInfo, sizeof(ClientInfo)); RTMPMoveMemory(ClientInfo.DevAddr, pClient->addr, MAC_ADDR_LEN); DBGPRINT(RT_DEBUG_INFO, (" -- InsertP2PGroupInfoTlv( ) (Mac %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(pClient->addr))); RTMPMoveMemory(ClientInfo.InterfaceAddr, pClient->InterfaceAddr, MAC_ADDR_LEN); DevCapability = pClient->DevCapability; if (P2P_TEST_FLAG(pClient, P2PFLAG_DEVICE_DISCOVERABLE)) DevCapability |= DEVCAP_CLIENT_DISCOVER; ClientInfo.Capability = DevCapability; config_method = cpu2be16(pClient->ConfigMethod); RTMPMoveMemory(ClientInfo.ConfigMethod, &config_method, 2); RTMPMoveMemory(ClientInfo.PrimaryDevType, pClient->PrimaryDevType, P2P_DEVICE_TYPE_LEN); ClientInfo.NumSecondaryType = pClient->NumSecondaryType; /* JANTEMP set to zero first. will add soon. */ ClientInfo.NumSecondaryType = 0; RTMPMoveMemory(ClientInfo.PrimaryDevType, pClient->PrimaryDevType, P2P_DEVICE_TYPE_LEN); ClientInfo.Length = SIZE_OF_FIXED_CLIENT_INFO_DESC - 1 + pClient->DeviceNameLen + 4; RTMPMoveMemory(pContent, &ClientInfo, SIZE_OF_FIXED_CLIENT_INFO_DESC); pContent += SIZE_OF_FIXED_CLIENT_INFO_DESC; /* Length is accumulated length for this attirbute. */ Length += SIZE_OF_FIXED_CLIENT_INFO_DESC; /* Insert WPS Device Name TLV */ *((PUSHORT) pContent) = cpu2be16(WSC_ID_DEVICE_NAME); *((PUSHORT) (pContent + 2)) = cpu2be16(pClient->DeviceNameLen); RTMPMoveMemory(pContent + 4, pClient->DeviceName, pClient->DeviceNameLen); Length += (UCHAR)pClient->DeviceNameLen + 4; pContent += (pClient->DeviceNameLen + 4); /* Assign this client info descriptor's length. Length is accumulated length. so can't use Length. */ /*if (!RTMPEqualMemory(pClient->SecondaryDevType, &ZeroType, P2P_DEVICE_TYPE_LEN)) { // JANTEMP. force to support add only one secondary device type first. will improve later. *pContent = 1; RTMPMoveMemory(pContent+1, pClient->SecondaryDevType, P2P_DEVICE_TYPE_LEN); Length += (1+P2P_DEVICE_TYPE_LEN); ClientInfo.Length += (1+P2P_DEVICE_TYPE_LEN); }*/ DBGPRINT(RT_DEBUG_INFO, (" ----- InsertP2PGroupInfoTlv(%d) (Total Len now = %d) DevNameLen = %ld.\n", i, Length, pClient->DeviceNameLen)); } } /* Because the length field doesn't count itself. So when update Attribute length, need to add number of client descriptor. */ //Length += NoOfClient; *(pDest + 1) = Length; /* Set to real length */ return (Length + 3); } /* ========================================================================== Description: Used to insert P2P subelement TLV format. Parameters: pInBuffer : pointer to data that contains data to put in pOutBuffer : pointer to buffer that should put data to. Note: ========================================================================== */ ULONG InsertP2PSubelmtTlv( IN PRTMP_ADAPTER pAd, IN UCHAR SubId, IN PUCHAR pInBuffer, IN PUCHAR pOutBuffer) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PUCHAR pDest; ULONG Length; pDest = pOutBuffer; RTMPZeroMemory(pDest, 255); *pDest = SubId; pDest += 1; Length = 0; switch(SubId) { case SUBID_P2P_INVITE_FLAG: *pDest = 1; *(pDest + 1) = 0; *(pDest + 2) = *pInBuffer; Length = 4; break; case SUBID_P2P_STATUS: /* 0 */ *pDest = 1; *(pDest + 1) = 0; *(pDest + 2) = *pInBuffer; Length = 4; break; case SUBID_P2P_MINOR_REASON: /* 0 */ break; case SUBID_P2P_CAP: /* 2 */ *pDest = 2; *(pDest + 1) = 0; RTMPMoveMemory(pDest + 2, pInBuffer, 2); Length = 5; break; case SUBID_P2P_DEVICE_ID: /* 3 this is Device Address! */ case SUBID_P2P_GROUP_BSSID: /* 6 group Bssid */ case SUBID_P2P_INTERFACE_ADDR: /* 9 */ *pDest = 6; *(pDest + 1) = 0; RTMPMoveMemory(pDest + 2, pInBuffer, MAC_ADDR_LEN); /* DEvice Type */ /*RTMPMoveMemory(pDest + 7, pAd->CurrentAddress, 8); */ Length = 9; break; case SUBID_P2P_OWNER_INTENT: /* 4 */ *pDest = 1; *(pDest + 1) = 0; *(pDest + 2) = *pInBuffer; Length = 4; break; case SUBID_P2P_CONFIG_TIMEOUT: /* 5 */ *pDest = 2; *(pDest + 1) = 0; RTMPMoveMemory(pDest + 2, pInBuffer, 2); Length = 5; break; case SUBID_P2P_CHANNEL_LIST: /* 11 */ *(pDest + 1) = 0; *(pDest + 2) = pAd->CommonCfg.CountryCode[0]; *(pDest + 3) = pAd->CommonCfg.CountryCode[1]; *(pDest + 4) = 0x04; InsertP2pChannelList(pAd, *pInBuffer, &Length, (pDest + 5)); *pDest = Length + 3; Length += 6; break; case SUBID_P2P_OP_CHANNEL: case SUBID_P2P_LISTEN_CHANNEL: /* 6 */ *pDest = 5; *(pDest + 1) = 0; /*Annex J. 802.11ERVmb_D3/0.pdf */ *(pDest + 2) = pAd->CommonCfg.CountryCode[0]; *(pDest + 3) = pAd->CommonCfg.CountryCode[1]; *(pDest + 4) = 0x04; *(pDest + 5) = ChannelToClass(*pInBuffer, 1/*COUNTRY_USA*/); *(pDest + 6) = *pInBuffer; Length = 8; break; case SUBID_P2P_EXT_LISTEN_TIMING: /* 8 */ *pDest = 4; *(pDest + 1) = 0; *((PUSHORT) (pDest + 2)) = (P2P_EXT_LISTEN_PERIOD); *((PUSHORT) (pDest + 4)) = (P2P_EXT_LISTEN_INTERVAL); Length = 7; break; case SUBID_P2P_MANAGEABILITY: /* 10 */ break; case SUBID_P2P_NOA: /* 12 */ break; case SUBID_P2P_DEVICE_INFO: /* 13 */ *pDest = 17 + pP2PCtrl->DeviceNameLen + 4 + (8*pP2PCtrl->DevInfo.SecDevTypList[0]); *(pDest + 1) = 0; RTMPMoveMemory(pDest + 2, pInBuffer, MAC_ADDR_LEN); /* DEvice Type */ *((PUSHORT) (pDest + 8)) = cpu2be16(pP2PCtrl->ConfigMethod); RTMPMoveMemory(pDest + 10, &pP2PCtrl->DevInfo.PriDeviceType[0], 8); { *(pDest + 18) = pP2PCtrl->DevInfo.SecDevTypList[0]; NdisMoveMemory((pDest + 19), &pP2PCtrl->DevInfo.SecDevTypList[1], (8*(pP2PCtrl->DevInfo.SecDevTypList[0]))); pDest += (19 + (8*pP2PCtrl->DevInfo.SecDevTypList[0])); } *((PUSHORT) pDest) = cpu2be16(WSC_ID_DEVICE_NAME); *((PUSHORT) (pDest + 2)) = cpu2be16(pP2PCtrl->DeviceNameLen); RTMPMoveMemory(pDest + 4, &pP2PCtrl->DeviceName[0], pP2PCtrl->DeviceNameLen); Length = 20+ (8*(pP2PCtrl->DevInfo.SecDevTypList[0])) + pP2PCtrl->DeviceNameLen + 4; break; case SUBID_P2P_GROUP_INFO: /* 14 */ break; case SUBID_P2P_GROUP_ID: /* 15 */ RTMPMoveMemory(pDest, &pInBuffer[0], (pInBuffer[0] + 2)); Length = 1 + (pInBuffer[0] + 2); DBGPRINT(RT_DEBUG_ERROR, (" -----Insert SUBID_P2P_GROUP_ID (Len = %ld) \n", Length)); break; case SUBID_P2P_INTERFACE: /* 16 */ break; default: *pDest = 0; Length = 0; break; } return Length; } /* ========================================================================== Description: Used to insert P2P Channel list attribute. Parameters: ChannelListLen : output for total length. pDest : pointer to buffer that should put data to. Note: ========================================================================== */ VOID InsertP2pChannelList( IN PRTMP_ADAPTER pAd, IN UCHAR OpChannel, OUT ULONG *ChannelListLen, OUT PUCHAR pDest) { UCHAR i, pos; UCHAR LastRegClass = 0xff, CurRegClass; PUCHAR pLastLenPos; UCHAR LastNum = 0; pLastLenPos = pDest + 1; pos = 2; for (i = 0; i < pAd->ChannelListNum; i++) { CurRegClass = ChannelToClass(pAd->ChannelList[i].Channel, 1); /* 0. Decide current regulatory class. 11y */ /* Insert RegClass if necessary */ if (LastRegClass == 0xff) { /* case 0 : initilize */ LastRegClass = CurRegClass; *(pDest) = CurRegClass; } else if ((CurRegClass != LastRegClass) && (LastRegClass != 0xff)) { /* case 1 : change regulatory class */ *(pDest + pos) = CurRegClass; *pLastLenPos = LastNum; LastNum = 0; LastRegClass = CurRegClass; pLastLenPos = pDest + pos + 1; pos = pos + 2; } /* Insert */ *(pDest + pos) = pAd->ChannelList[i].Channel; LastNum++; pos++; /* Last item. Update Num. */ if (i == pAd->ChannelListNum -1) { *pLastLenPos = LastNum; } } *ChannelListLen = pos; DBGPRINT(RT_DEBUG_TRACE, ("%s:: Channel List ==> output length is %ld \n", __FUNCTION__, *ChannelListLen)); for (i = 0; i <*ChannelListLen; ) { DBGPRINT(RT_DEBUG_TRACE, (": %x %x %x %x %x %x %x %x %x \n", *(pDest+i), *(pDest+i+1), *(pDest+i+2), *(pDest+i+3), *(pDest+i+4), *(pDest+i+5), *(pDest+i+6), *(pDest+i+7), *(pDest+i+8))); i = i + 9; } } /* ========================================================================== Description: Parse P2P subelement content to fill into the correct output buffer Parameters: return : TRUE if the pSearchAddr is in GroupInfoAttribute. Note: ========================================================================== */ BOOLEAN P2pParseGroupInfoAttribute( IN PRTMP_ADAPTER pAd, IN UCHAR P2pindex, IN VOID *Msg, IN ULONG MsgLen) { PEID_STRUCT pEid; ULONG Length = 0; LONG GroupInfoLen; UCHAR i; UCHAR idx; PUCHAR pData; UCHAR ThisDescLen; BOOLEAN brc = TRUE; P2P_CLIENT_INFO_DESC *pClient; USHORT WscType, WscLen; ULONG LeftLength; PP2PEID_STRUCT pP2pEid; ULONG AttriLen; UCHAR SmallerP2Pidx; BOOLEAN bSendP2pEvent = FALSE; pEid = (PEID_STRUCT) Msg; Length = 0; LeftLength = MsgLen; pEid = (PEID_STRUCT)Msg; while ((ULONG)(pEid->Len + 2) <= LeftLength) { /* might contains P2P IE and WPS IE. So use if else if enough for locate P2P IE. */ /* To check Octet[1] is because Ralink add one byte itself for P2P IE. So the IE content shift one byte afterward. */ if (RTMPEqualMemory(&pEid->Octet[0], WIFIDIRECT_OUI, 4)) { /* To check Octet[5] for first P2PEid */ /* is because Ralink add one byte itself for P2P IE. So the IE content shift one byte afterward. */ pP2pEid = (PP2PEID_STRUCT) &pEid->Octet[4]; AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; Length = 0; while ((Length + 3 + AttriLen) <= pEid->Len) { switch(pP2pEid->Eid) { case SUBID_P2P_GROUP_INFO: GroupInfoLen = AttriLen; pData = &pP2pEid->Octet[0]; DBGPRINT(RT_DEBUG_INFO, ("SUBID_P2P_GROUP_INFO - Go index=%d\n", P2pindex)); while(GroupInfoLen > 0) { pClient = (P2P_CLIENT_INFO_DESC*)pData; ThisDescLen = pClient->Length; if ((ThisDescLen < 23) || (ThisDescLen > GroupInfoLen)) { DBGPRINT(RT_DEBUG_ERROR, ("Error parsing P2P IE group info attribute. This Client -%d/%ld\n", ThisDescLen, GroupInfoLen)); break; } idx = P2pGroupTabSearch(pAd, pClient->DevAddr); if ((idx < MAX_P2P_GROUP_SIZE) && (pAd->P2pTable.Client[idx].P2pClientState != P2PSTATE_DISCOVERY_CLIENT) && (pAd->P2pTable.Client[idx].P2pClientState != P2PSTATE_CLIENT_DISCO_COMMAND)) { /* P2P topology changed. Reset the P2P Table and rescan. */ if (idx > P2pindex) SmallerP2Pidx = P2pindex + 1; /* don't delete GO */ else SmallerP2Pidx = idx; /* this method also delete GO. :( */ DBGPRINT(RT_DEBUG_TRACE, ("!P2P topology changed[P2pClientState = %d] when parsing P2P IE group info attribute. Delete from index : %d\n", pAd->P2pTable.Client[idx].P2pClientState, SmallerP2Pidx )); /*P2PPrintP2PEntry(pAd, idx); */ for (i = (SmallerP2Pidx); i < MAX_P2P_GROUP_SIZE;i++) { if ((pAd->P2pTable.Client[i].P2pClientState == P2PSTATE_PROVISION_COMMAND) || (pAd->P2pTable.Client[i].P2pClientState == P2PSTATE_CONNECT_COMMAND) || (pAd->P2pTable.Client[i].P2pClientState > P2PSTATE_DISCOVERY_CLIENT)) { DBGPRINT(RT_DEBUG_ERROR, ("!break right away because we have another connect command to continue. update topology is not so important to do right now.\n" )); break; } else { P2pGroupTabDelete(pAd, i, pAd->P2pTable.Client[i].addr); } /* Don't update this now. */ if (pAd->P2pTable.ClientNumber == SmallerP2Pidx) return FALSE; } /* because we check idx in following code, */ /* So need to Search again after topology changed */ idx = P2pGroupTabSearch(pAd, pClient->DevAddr); } if ((idx == P2P_NOT_FOUND) && ( pAd->P2pTable.ClientNumber < (MAX_P2P_GROUP_SIZE - 1)) && (!RTMPEqualMemory(pClient->DevAddr, pAd->P2pCfg.CurrentAddress, MAC_ADDR_LEN))) { idx = P2pGroupTabInsert(pAd, pClient->DevAddr, P2PSTATE_DISCOVERY_CLIENT, NULL, 0, 0, 0); DBGPRINT(RT_DEBUG_TRACE, ("Insert a P2P Client to P2P table [%d] .\n", idx)); /* Insert a P2P Client followi ng the GO that we received. Need to delete ALL following client that is already in P2P table. */ /* Because the number of client that is in the p2p group might be changed. */ bSendP2pEvent = TRUE; } if (idx < MAX_P2P_GROUP_SIZE) { pAd->P2pTable.Client[idx].DevCapability = pClient->Capability; /* Don't update state when it's in P2PSTATE_CLIENT_DISCO_COMMAND */ if (pAd->P2pTable.Client[idx].P2pClientState != P2PSTATE_CLIENT_DISCO_COMMAND) pAd->P2pTable.Client[idx].P2pClientState = P2PSTATE_DISCOVERY_CLIENT; pAd->P2pTable.Client[idx].ConfigMethod = *((PUSHORT) pClient->ConfigMethod); pAd->P2pTable.Client[idx].ConfigMethod = be2cpu16(pAd->P2pTable.Client[idx].ConfigMethod); RTMPMoveMemory(pAd->P2pTable.Client[idx].PrimaryDevType, pClient->PrimaryDevType, P2P_DEVICE_TYPE_LEN); pAd->P2pTable.Client[idx].NumSecondaryType = pClient->NumSecondaryType; WscType = cpu2be16(*((PUSHORT) &pClient->Octet[pClient->NumSecondaryType*P2P_DEVICE_TYPE_LEN])); WscLen = cpu2be16(*((PUSHORT) (&pClient->Octet[2 + pClient->NumSecondaryType*P2P_DEVICE_TYPE_LEN]))); RTMPMoveMemory(&pAd->P2pTable.Client[idx].DeviceName[0], &pClient->Octet[4+(pClient->NumSecondaryType*P2P_DEVICE_TYPE_LEN)], 32); if (WscLen <= 32) pAd->P2pTable.Client[idx].DeviceNameLen = WscLen; pAd->P2pTable.Client[idx].MyGOIndex = P2pindex; pAd->P2pTable.Client[idx].DeviceName[pAd->P2pTable.Client[idx].DeviceNameLen] = 0x0; #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT if (bSendP2pEvent) P2pSendWirelessEvent(pAd, RT_P2P_DEVICE_FIND, &pAd->P2pTable.Client[idx], NULL); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ if (pAd->P2pCfg.ConnectingIndex < MAX_P2P_GROUP_SIZE) { if (RTMPEqualMemory(pAd->P2pTable.Client[idx].addr, &pAd->P2pCfg.ConnectingMAC[0], MAC_ADDR_LEN)) P2pConnectAfterScan(pAd, FALSE, idx); } } GroupInfoLen -= (ThisDescLen + 1); pData += (ThisDescLen + 1); }; break; default: break; } Length = Length + 3 + AttriLen; /* Eid[1] + Len[1]+ content[Len] */ pP2pEid = (PP2PEID_STRUCT)((UCHAR*)pP2pEid + 3 + AttriLen); AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; } /* We already get what we want. so break. */ break; } LeftLength = LeftLength - pEid->Len - 2; pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } return brc; } /* ========================================================================== Description: Parse P2P NoA subelement content to make appropriate action for NoA schedule. Parameters: Note: ========================================================================== */ VOID P2pParseNoASubElmt( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, IN UCHAR wcidindex, IN UINT32 Sequence) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; ULONG Length = 0; PP2PEID_STRUCT pP2pEid; ULONG AttriLen; ULONG LeftLength; PEID_STRUCT pEid; BOOLEAN brc; BOOLEAN bNoAAttriExist = FALSE; PUCHAR pPtrEid = NULL; /* Intel sends multiple P2P IE... So I can't give each input a default value.. */ if (MsgLen == 0) return; LeftLength = MsgLen; pEid = (PEID_STRUCT)Msg; while ((ULONG)(pEid->Len + 2) <= LeftLength) { /* might contains P2P IE and WPS IE. So use if else if enough for locate P2P IE. */ if (RTMPEqualMemory(&pEid->Octet[0], WIFIDIRECT_OUI, 4)) { /* Get Request content capability */ pP2pEid = (PP2PEID_STRUCT) &pEid->Octet[4]; pPtrEid = (PUCHAR) pP2pEid; AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; Length = 0; while ((Length + 3 + AttriLen) <= pEid->Len) { switch(pP2pEid->Eid) { case SUBID_P2P_NOA: { PUCHAR pData = &pEid->Octet[0]; DBGPRINT(RT_DEBUG_INFO,("Get NoA Attr: %x %x %x %x %x %x %x %x %x \n", *(pData+0), *(pData+1), *(pData+2), *(pData+3), *(pData+4), *(pData+5), *(pData+6), *(pData+7), *(pData+8))); bNoAAttriExist = TRUE; brc = P2pHandleNoAAttri(pAd, &pAd->MacTab.Content[wcidindex], pPtrEid); /* Got a NoA Attribute from this p2pindex. In fact, This should be GO. */ if (brc == TRUE) pP2PCtrl->NoAIndex = wcidindex; } break; default: break; } Length = Length + 3 + AttriLen; /* Eid[1] + Len[1]+ content[Len] */ pP2pEid = (PP2PEID_STRUCT)((UCHAR*)pP2pEid + 3 + AttriLen); pPtrEid = (PUCHAR) pP2pEid; AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; } } LeftLength = LeftLength - pEid->Len - 2; pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } if (bNoAAttriExist == FALSE) { if (P2P_TEST_BIT(pAd->P2pCfg.CTWindows, P2P_OPPS_BIT)) { DBGPRINT(RT_DEBUG_TRACE,("Beacon and no NoA Attribute! \n")); P2pStopOpPS(pAd); } if ((pAd->MacTab.Content[wcidindex].P2pInfo.NoADesc[0].bValid == TRUE)) { DBGPRINT(RT_DEBUG_TRACE,("Beacon and no NoA Attribute!Stop active NoA [%d]\n", Sequence)); P2pStopNoA(pAd, &pAd->MacTab.Content[wcidindex]); } } } /* ========================================================================== Description: Parse P2P NoA subelement content to make appropriate action for NoA schedule. Parameters: Note: ========================================================================== */ VOID P2pParseExtListenSubElmt( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT USHORT *ExtListenPeriod, OUT USHORT *ExtListenInterval) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; ULONG Length = 0; PP2PEID_STRUCT pP2pEid; ULONG AttriLen; /*UCHAR offset; */ ULONG LeftLength; PEID_STRUCT pEid; USHORT ExtTime; /*BOOLEAN brc; */ /*PUCHAR pData; */ /* Intel sends multiple P2P IE... So I can't give each input a default value.. */ if (MsgLen == 0) return; LeftLength = MsgLen; pEid = (PEID_STRUCT)Msg; while ((ULONG)(pEid->Len + 2) <= LeftLength) { /* might contains P2P IE and WPS IE. So use if else if enough for locate P2P IE. */ if (RTMPEqualMemory(&pEid->Octet[0], WIFIDIRECT_OUI, 4)) { /* Get Request content capability */ pP2pEid = (PP2PEID_STRUCT) &pEid->Octet[4]; AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; Length = 0; while ((Length + 3 + AttriLen) <= pEid->Len) { switch(pP2pEid->Eid) { case SUBID_P2P_EXT_LISTEN_TIMING: if (AttriLen == 4) { ExtTime = *(PUSHORT)&pP2pEid->Octet[0]; DBGPRINT(RT_DEBUG_TRACE, (" - (Ext Listen Period %x = %d) \n", ExtTime, ExtTime)); if (ExtTime > 0) pP2PCtrl->ExtListenPeriod = ExtTime; ExtTime = *(PUSHORT)&pP2pEid->Octet[2]; DBGPRINT(RT_DEBUG_TRACE, (" - (Ext Listen Interval %x = %d) \n", ExtTime, ExtTime)); if (ExtTime > 0) pP2PCtrl->ExtListenInterval = ExtTime; } break; default: break; } Length = Length + 3 + AttriLen; /* Eid[1] + Len[1]+ content[Len] */ pP2pEid = (PP2PEID_STRUCT)((UCHAR*)pP2pEid + 3 + AttriLen); AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; } } LeftLength = LeftLength - pEid->Len - 2; pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } } /* ========================================================================== Description: Parse P2P subelement content to fill into the correct output buffer Parameters: Note: ========================================================================== */ VOID P2pParseManageSubElmt( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT UCHAR *pChannel, OUT UCHAR *pNumOfP2pOtherAttribute, OUT UCHAR *pTotalNumOfP2pAttribute, OUT UCHAR *pMamageablity, OUT UCHAR *pMinorReason) { PP2PEID_STRUCT pP2pEid; PEID_STRUCT pEid; ULONG Length; ULONG AttriLen; ULONG LeftLength; DBGPRINT(RT_DEBUG_TRACE, ("P2pParseManageSubElmt MsgLen = %ld. \n", MsgLen)); if (pNumOfP2pOtherAttribute != NULL) *pNumOfP2pOtherAttribute = 0; if (pTotalNumOfP2pAttribute != NULL) *pTotalNumOfP2pAttribute = 0; LeftLength = MsgLen; pEid = (PEID_STRUCT)Msg; while ((ULONG)(pEid->Len + 2) <= LeftLength) { if (pEid->Eid == IE_CHANNEL_USAGE) { *pChannel = pEid->Octet[2]; DBGPRINT(RT_DEBUG_TRACE, ("IE_CHANNEL_USAGE = %x %x %x %x [ch=]%x. \n", pEid->Eid, pEid->Len, pEid->Octet[0], pEid->Octet[1], pEid->Octet[2])); } /* might contains P2P IE and WPS IE. So use if else if enough for locate P2P IE. */ if (RTMPEqualMemory(&pEid->Octet[0], WIFIDIRECT_OUI, 4)) { /* Get Request content capability */ pP2pEid = (PP2PEID_STRUCT) &pEid->Octet[4]; AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; Length = 0; while ((Length + 3 + AttriLen) <= pEid->Len) { switch(pP2pEid->Eid) { case SUBID_P2P_MINOR_REASON: DBGPRINT(RT_DEBUG_TRACE, ("SYNC - Has P2P SUBID_P2P_MINOR_REASON IE Minor Reason = %d.\n", pP2pEid->Octet[0])); if (pTotalNumOfP2pAttribute != NULL) *pTotalNumOfP2pAttribute = *pTotalNumOfP2pAttribute+1; if (pNumOfP2pOtherAttribute != NULL) *pNumOfP2pOtherAttribute = *pNumOfP2pOtherAttribute+1; if (pMinorReason != NULL) *pMinorReason = pP2pEid->Octet[0]; break; case SUBID_P2P_MANAGEABILITY: if (pTotalNumOfP2pAttribute != NULL) { *pTotalNumOfP2pAttribute = *pTotalNumOfP2pAttribute+1; DBGPRINT(RT_DEBUG_TRACE, ("SYNC -Ap Has P2P Manageability IE . Total P2P IE count is %d \n", *pTotalNumOfP2pAttribute)); } if (pMamageablity != NULL) *pMamageablity = pP2pEid->Octet[0]; break; default: if (pTotalNumOfP2pAttribute != NULL) *pTotalNumOfP2pAttribute = *pTotalNumOfP2pAttribute+1; if (pNumOfP2pOtherAttribute != NULL) *pNumOfP2pOtherAttribute = *pNumOfP2pOtherAttribute+1; break; } Length = Length + 3 + AttriLen; /* Eid[1] + Len[1]+ content[Len] */ pP2pEid = (PP2PEID_STRUCT)((UCHAR*)pP2pEid + 3 + AttriLen); AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; } } LeftLength = LeftLength - pEid->Len - 2; pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } } /* ========================================================================== Description: Parse P2P subelement content to fill into the correct output buffer. May contain multiple P2P IE and WPS IE. So need while loop to find all IE. Parameters: Note: ========================================================================== */ VOID P2pParseSubElmt( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, IN BOOLEAN bBeacon, OUT USHORT *pDpid, OUT UCHAR *pGroupCap, OUT UCHAR *pDeviceCap, OUT UCHAR *pDeviceName, OUT UCHAR *pDeviceNameLen, OUT UCHAR *pDevAddr, OUT UCHAR *pInterFAddr, OUT UCHAR *pBssidAddr, OUT UCHAR *pSsidLen, OUT UCHAR *pSsid, OUT USHORT *pConfigMethod, OUT USHORT *pWpsConfigMethod, OUT UCHAR *pDevType, OUT UCHAR *pListenChannel, OUT UCHAR *pOpChannel, OUT UCHAR *pChannelList, OUT UCHAR *pIntent, OUT UCHAR *pStatusCode, OUT UCHAR *pInviteFlag, #ifdef WFD_SUPPORT OUT ULONG *pWfdSubelementLen, OUT PUCHAR pWfdSubelement, #endif /* WFD_SUPPORT */ OUT VOID *pPrivate) { ULONG Length = 0; PP2PEID_STRUCT pP2pEid; ULONG AttriLen; USHORT WscType, WscLen; UCHAR offset; PEID_STRUCT pEid; UCHAR ChannelListLeft = MAX_NUM_OF_CHANNELS; /*USHORT ExtTime; */ ULONG AccuP2PIELen; ULONG AccuIeLen = 0; //for Beacon lost on SCC debug! ULONG ChannelAttriLen = 0; ULONG NumOfChannel = 0; UCHAR Choffset =0; /* Intel sends multiple P2P IE... So I can't give each input a default value.. */ if (MsgLen == 0) return; pEid = (PEID_STRUCT)Msg; AccuIeLen = pEid->Len + 2; /* 2: Tag (1 octet) + Len (1 octet) */ if (RTMPEqualMemory(&pEid->Octet[1], WIFIDIRECT_OUI, 4)) { /* In this case, length is 2 bytes. */ AccuIeLen = pEid->Len + pEid->Octet[0]*256 + 3; /* 3: Tag (1 octet) + Len (2 octet) */ } while ((ULONG)(AccuIeLen) <= MsgLen) { if (RTMPEqualMemory(&pEid->Octet[0], WPS_OUI, 4)) { /*PUCHAR pWscEid = &pEid->Eid;*/ if (bBeacon == TRUE) P2PParseWPSIE(&pEid->Octet, (pEid->Len + 2), pDpid, pWpsConfigMethod, pDeviceName, pDeviceNameLen); else P2PParseWPSIE(&pEid->Octet, (pEid->Len + 2), pDpid, pWpsConfigMethod, NULL, NULL); } /* might contains P2P IE and WPS IE. So use if else if enough for locate P2P IE. */ else if ((RTMPEqualMemory(&pEid->Octet[0], WIFIDIRECT_OUI, 4)) ||(RTMPEqualMemory(&pEid->Octet[1], WIFIDIRECT_OUI, 4))) { /* Get Request content capability */ if (RTMPEqualMemory(&pEid->Octet[1], WIFIDIRECT_OUI, 4)) { /* In this case, length is 2 bytes. */ pP2pEid = (PP2PEID_STRUCT) &pEid->Octet[5]; AccuP2PIELen = pEid->Len + pEid->Octet[0]*256; } else { pP2pEid = (PP2PEID_STRUCT) &pEid->Octet[4]; AccuP2PIELen = pEid->Len; } /* The value of AccuP2PIELen shall reduce the length of OUI (4). */ AccuP2PIELen -= 4; AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] *8; Length = 0; while ((Length + 3 + AttriLen) <= AccuP2PIELen) { switch(pP2pEid->Eid) { case SUBID_P2P_EXT_LISTEN_TIMING: break; case SUBID_P2P_INVITE_FLAG: if (pInviteFlag != NULL) *pInviteFlag = pP2pEid->Octet[0]; break; case SUBID_P2P_MANAGEABILITY: break; case SUBID_P2P_CAP: *pGroupCap = pP2pEid->Octet[1]; *pDeviceCap = pP2pEid->Octet[0]; break; case SUBID_P2P_OWNER_INTENT: if (pIntent != NULL) *pIntent = pP2pEid->Octet[0]; break; case SUBID_P2P_CHANNEL_LIST: DBGPRINT(RT_DEBUG_INFO, ("SUBID_P2P_CHANNEL_LIST - ( Len %d %d)= %x %x %x %x %x %x..\n", pP2pEid->Len[0], pP2pEid->Len[1], pP2pEid->Octet[0],pP2pEid->Octet[1],pP2pEid->Octet[2],pP2pEid->Octet[3],pP2pEid->Octet[4],pP2pEid->Octet[5])); if (pChannelList != NULL && (AttriLen -3 > 0)) /* reduce Country String 3 bytes*/ { ChannelAttriLen = AttriLen; ChannelAttriLen -= 3; /* reduce Country String 3 bytes*/ Choffset +=3; while(ChannelAttriLen > 0 && (ChannelListLeft - NumOfChannel)> 0 && NumOfChannel < 20) { Choffset +=1; /* skip operating class(1) */ NumOfChannel = pP2pEid->Octet[Choffset]; RTMPMoveMemory(pChannelList, &pP2pEid->Octet[Choffset+1], NumOfChannel); /* copy n channels*/ pChannelList += NumOfChannel; Choffset +=1; /* Num of channels(1) */ Choffset += NumOfChannel ; /* Channel Lis(n) */ ChannelListLeft -=NumOfChannel; ChannelAttriLen -=(2+NumOfChannel); /* operating class(1) + Num of channels(1) + Channel Lis(n) */ } } break; case SUBID_P2P_OP_CHANNEL: if (pOpChannel != NULL) *pOpChannel = pP2pEid->Octet[4]; /* Octet[1] is regulatory */ break; case SUBID_P2P_LISTEN_CHANNEL: if (pListenChannel != NULL) *pListenChannel = pP2pEid->Octet[4]; /* Octet[1] is regulatory */ break; case SUBID_P2P_GROUP_BSSID: /* 6 group Bssid */ if (pBssidAddr != NULL) RTMPMoveMemory(pBssidAddr, &pP2pEid->Octet[0], MAC_ADDR_LEN); break; case SUBID_P2P_INTERFACE_ADDR: /* 9 */ if (pInterFAddr != NULL) { RTMPMoveMemory(pInterFAddr, &pP2pEid->Octet[0], MAC_ADDR_LEN); } break; case SUBID_P2P_DEVICE_ID: /* Beacon has this field. */ if (pDevAddr != NULL) RTMPMoveMemory(pDevAddr, &pP2pEid->Octet[0], MAC_ADDR_LEN); break; case SUBID_P2P_GROUP_ID: if ((pSsid != NULL) && (pP2pEid->Len[0]) > 6 && (pP2pEid->Len[0] <= 38)) { RTMPMoveMemory(pSsid, &pP2pEid->Octet[6], (pP2pEid->Len[0] - 6)); if (pSsidLen != NULL) { *pSsidLen = pP2pEid->Len[0] - 6; DBGPRINT(RT_DEBUG_INFO, (" SUBID_P2P_GROUP_ID - SSID ( Len %d)= %c %c %c %c %c %c %c %c %c.. \n", *pSsidLen, pSsid[0], pSsid[1],pSsid[2],pSsid[3],pSsid[4],pSsid[5],pSsid[6],pSsid[7],pSsid[8])); } } break; case SUBID_P2P_DEVICE_INFO: if (pDevAddr != NULL) RTMPMoveMemory(pDevAddr, &pP2pEid->Octet[0], MAC_ADDR_LEN); if (pConfigMethod != NULL) *pConfigMethod = be2cpu16(*(PUSHORT)&pP2pEid->Octet[6]); if (pDevType != NULL) RTMPMoveMemory(pDevType, &pP2pEid->Octet[8], 8); /* Count the DeviceName offset. */ offset = 17 + pP2pEid->Octet[16] * 8; WscType = cpu2be16(*((PUSHORT) &pP2pEid->Octet[offset])); WscLen = cpu2be16(*((PUSHORT) (&pP2pEid->Octet[offset+2]))); if ((WscType == WSC_ID_DEVICE_NAME) && (WscLen <= 32)) { if ((pDeviceName != NULL)) { *pDeviceNameLen = (UCHAR)WscLen; RTMPMoveMemory(pDeviceName, &pP2pEid->Octet[21 + pP2pEid->Octet[16]*8], WscLen); DBGPRINT(RT_DEBUG_INFO, ("SUBID_P2P_DEVICE_INFO Device Name= %c %c %c %c %c %c \n", pDeviceName[0], pDeviceName[1],pDeviceName[2],pDeviceName[3],pDeviceName[4],pDeviceName[5])); } } break; case SUBID_P2P_STATUS: if (pStatusCode != NULL) { *pStatusCode = pP2pEid->Octet[0]; DBGPRINT(RT_DEBUG_INFO, (" SUBID_P2P_STATUS Eid = %x \n", *pStatusCode)); } break; case SUBID_P2P_GROUP_INFO: break; case SUBID_P2P_NOA: break; case SUBID_P2P_CONFIG_TIMEOUT: break; default: DBGPRINT(RT_DEBUG_ERROR, (" SUBID_P2P_ unknown Eid = %x \n", pP2pEid->Eid)); break; } Length = Length + 3 + AttriLen; /* Eid[1] + Len[1]+ content[Len] */ if (Length >= AccuP2PIELen) break; pP2pEid = (PP2PEID_STRUCT)((UCHAR*)pP2pEid + 3 + AttriLen); AttriLen = pP2pEid->Len[0] + pP2pEid->Len[1] * 256; } } #ifdef WFD_SUPPORT else if (( /* Due to Intel p2p WiDi need to get rtsp port by ioctl, so leverage the WFD code base structure*/ (pAd->StaCfg.WfdCfg.bWfdEnable)) && NdisEqualMemory(pEid->Octet, WIFIDISPLAY_OUI, 4) && (pEid->Len >= 4)) { //hex_dump("WFD_IE", &pEid->Eid, pEid->Len +2); if (pWfdSubelement && pWfdSubelementLen) { RTMPMoveMemory(pWfdSubelement, &pEid->Eid, pEid->Len +2); *pWfdSubelementLen = pEid->Len + 2; } } #endif /* WFD_SUPPORT */ /* already reach the last IE. Stop finding next Eid. */ if (AccuIeLen >= MsgLen) break; /* Forward buffer to next pEid */ if (RTMPEqualMemory(&pEid->Octet[1], WIFIDIRECT_OUI, 4)) { pEid = (PEID_STRUCT)((UCHAR*)pEid + (pEid->Len + pEid->Octet[0]*256 + 3)); /* We already accumul ate all P2P IE. don't need to search next P2P IE */ break; } else { pEid = (PEID_STRUCT)((UCHAR*)pEid + pEid->Len + 2); } /* Since we get the next pEid, */ /* Predict the accumulated IeLen after adding the next pEid's length. */ /* The accumulated IeLen is for checking length. */ if (RTMPEqualMemory(&pEid->Octet[1], WIFIDIRECT_OUI, 4)) { AccuIeLen += (pEid->Len + pEid->Octet[0]*256 + 3); } else { AccuIeLen += (pEid->Len + 2); } } } /* ========================================================================== Description: The routine that is called when receiving Go Negociation Confirm packet. Parameters: Note: ========================================================================== */ VOID P2pReceGoNegoConfirmAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; UCHAR DevType[8] = {0}, DevAddr[6] = {0}, IfAddr[6] = {0}; UCHAR Channel, OpChannel = 0, Intent, index, Ssid[32]; UCHAR GroupCap, DeviceCap, DeviceNameLen; /*ULONG FrameLen; */ /*ULONG TempLen; */ UCHAR StatusCode, SsidLen = 0; /*PEID_STRUCT pEid; */ /*ULONG LeftLength; */ USHORT Dpid, ConfigMethod; UCHAR AllZero[MAC_ADDR_LEN] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; DBGPRINT(RT_DEBUG_ERROR, (" P2P - Recieve Confirm Confirm Confirm. -> From %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(pFrame->p80211Header.Addr2))); /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen + 7 - sizeof(P2P_PUBLIC_FRAME)), FALSE, &Dpid, &GroupCap, &DeviceCap, NULL, &DeviceNameLen, DevAddr, IfAddr, NULL, &SsidLen, (PUCHAR)&Ssid, &ConfigMethod, NULL, DevType, &Channel, &OpChannel, NULL, &Intent, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); /* confirm doesn't attach device addr in subelement. So copy from SA. */ RTMPMoveMemory(DevAddr, pFrame->p80211Header.Addr2, MAC_ADDR_LEN); DBGPRINT(RT_DEBUG_ERROR, (" DevAddr = %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(DevAddr))); DBGPRINT(RT_DEBUG_ERROR, (" OpChannel = %d. \n", OpChannel)); /* Check StatusCode. */ if (StatusCode != 0) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Ignore Go Negociation Confirm Status Code not Success. \n")); return; } else DBGPRINT(RT_DEBUG_ERROR, (" P2P - Recieve Confirm Confirm Confirm Success. \n")); /* Check Peer is in the valid state to receive Go Negociation Response. */ index = P2pGroupTabSearch(pAd, DevAddr); if (index == P2P_NOT_FOUND) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Ignore Go Negociation Confirm from Unknown device. \n")); P2PPrintP2PEntry(pAd, 0); return; } else { /* Existing peer stay in another state. Doesn't need respond the Go Negociation Request. */ if ((pP2PCtrl->P2PConnectState == P2P_ANY_IN_FORMATION_AS_GO) && (pAd->P2pTable.Client[index].P2pClientState != P2PSTATE_WAIT_GO_COMFIRM)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Existing peer stay in another state. = %d. return.\n", pAd->P2pTable.Client[index].P2pClientState)); return; } } if (index < MAX_P2P_GROUP_SIZE) { PRT_P2P_CLIENT_ENTRY pP2pEntry = &pAd->P2pTable.Client[index]; if (!NdisEqualMemory(AllZero, IfAddr, MAC_ADDR_LEN)) RTMPMoveMemory(pP2pEntry->InterfaceAddr, IfAddr, MAC_ADDR_LEN); DBGPRINT(RT_DEBUG_ERROR, (" pP2pEntry[%d]->rule = %s. GoIntent = %d. My intent is %d. \n", index, decodeMyRule(pP2pEntry->Rule), pP2pEntry->GoIntent, pP2PCtrl->GoIntentIdx)); /*DBGPRINT(RT_DEBUG_ERROR, ("---->P2P pP2PCtrl->SsidLength = %d. [%x %x %x..]\n", pP2PCtrl->SSIDLen, pP2PCtrl->SSID[0], pP2PCtrl->SSID[1], pP2PCtrl->SSID[2]));*/ if (SsidLen > 0) { RTMPMoveMemory(pP2pEntry->Ssid, Ssid, 32); pP2pEntry->SsidLen = SsidLen; } if (pP2PCtrl->P2PConnectState == P2P_ANY_IN_FORMATION_AS_CLIENT) { int i; /* I become Client */ pP2pEntry->Rule = P2P_IS_GO; pP2pEntry->P2pClientState = P2PSTATE_GO_WPS; pP2PCtrl->GroupChannel = OpChannel; pP2PCtrl->GroupOpChannel = OpChannel; DBGPRINT(RT_DEBUG_ERROR, (" P2P - I become ENROLLEE!GOGO Go's SsidLen = %d.!!\n", pP2pEntry->SsidLen)); DBGPRINT(RT_DEBUG_ERROR, (" P2P - I become ENROLLEE!GOGO Go's Ssid[%d] = ", pP2pEntry->SsidLen)); for (i=0; iSsidLen; i++) DBGPRINT(RT_DEBUG_ERROR, ("%c ", pP2pEntry->Ssid[i])); DBGPRINT(RT_DEBUG_ERROR, ("\n")); DBGPRINT(RT_DEBUG_ERROR, (" P2P - I become ENROLLEE!!GOGO Go's Bssid = %02x:%02x:%02x:%02x:%02x:%02x!!\n", PRINT_MAC(pP2PCtrl->Bssid))); RTMPZeroMemory(pP2PCtrl->SSID, 32); RTMPMoveMemory(pP2PCtrl->SSID, pP2pEntry->Ssid, pP2pEntry->SsidLen); pP2PCtrl->SSIDLen = pP2pEntry->SsidLen; DBGPRINT(RT_DEBUG_ERROR, (" P2P - I become ENROLLEE!!GOGO Go's SSID[%d] = %s!!\n", pP2PCtrl->SSIDLen, pP2PCtrl->SSID)); P2pGoNegoDone(pAd, pP2pEntry); } else { /* I become GO */ pP2pEntry->Rule = P2P_IS_CLIENT; pP2PCtrl->GroupOpChannel = pP2PCtrl->GroupChannel; pP2pEntry->P2pClientState = P2PSTATE_GOT_GO_COMFIRM; P2pGoNegoDone(pAd, pP2pEntry); DBGPRINT(RT_DEBUG_ERROR, (" P2P - I become Internal REGISTRA!! REGISTRA. !! GOGOGO\n")); #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_CONNECTED, pP2pEntry, pP2PCtrl->CurrentAddress);//to inform UI, Group Started and as GO. #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ } } } /* ========================================================================== Description: The routine that is called when receiving Go Negociation Response packet. Parameters: Note: ========================================================================== */ VOID P2pReceGoNegoRspAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)&Elem->Msg[0]; UCHAR DevType[8], Ssid[32], DevAddr[6], IfAddr[6] = {0}, Channel = 0, OpChannel = 0, Intent, index; UCHAR GroupCap, DeviceCap, DeviceName[32]; UCHAR SsidLen = 0, DeviceNameLen = 0; PRT_P2P_CLIENT_ENTRY pP2pEntry; UCHAR StatusCode = 8; UCHAR TempIntent; USHORT Dpid, ConfigMethod; BOOLEAN Cancelled; UCHAR AllZero[MAC_ADDR_LEN] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; DBGPRINT(RT_DEBUG_ERROR, (" P2P - GOGOGO Recieve Response.Response MsgLen = %ld.\n", Elem->MsgLen)); if (pP2PCtrl->bP2pReSendTimerRunning) { pP2PCtrl->bP2pReSendTimerRunning = FALSE; RTMPCancelTimer(&pP2PCtrl->P2pReSendTimer, &Cancelled); } /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, &Dpid, &GroupCap, &DeviceCap, DeviceName, &DeviceNameLen, DevAddr, IfAddr, NULL, &SsidLen, Ssid, &ConfigMethod, NULL, DevType, &Channel, &OpChannel, NULL, &TempIntent, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); Intent = TempIntent >>1; DBGPRINT(RT_DEBUG_TRACE, (" Dev Addr = %02x:%02x:%02x:%02x:%02x:%02x. Intent = %d. My Intent = %d. \n", PRINT_MAC(DevAddr), Intent, pP2PCtrl->GoIntentIdx)); DBGPRINT(RT_DEBUG_TRACE, (" interface addr = %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(IfAddr))); DBGPRINT(RT_DEBUG_TRACE, (" P2P -StatusCode = %d. PeerIntent = %d . OpChannel = %d. My Intent = %d.\n", StatusCode, Intent, OpChannel, pP2PCtrl->GoIntentIdx)); /* Check StatusCode. */ if (StatusCode == P2PSTATUS_BOTH_INTENT15) { pP2PCtrl->P2pCounter.DisableRetryGrpFormCounter = 1200; DBGPRINT(RT_DEBUG_TRACE, (" P2P - Receive Status Code that both Go Intent Value is 15. = %x \n", StatusCode)); #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_GO_NEGO_FAIL_INTENT, NULL, DevAddr); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ P2pLinkDown(pAd, P2P_CONNECT_FAIL); return; } else if ((StatusCode == P2PSTATUS_REJECT_BY_USER) || (StatusCode == P2PSTATUS_PASSED)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P -Go Negociation Response Status Code PASSED or being rejected = %d \n", StatusCode)); index = P2pGroupTabSearch(pAd, DevAddr); if (index < MAX_P2P_GROUP_SIZE) { pAd->P2pTable.Client[index].ReTransmitCnt = 0; /* since peer return that info unknown, then stop resend GO Negociation Req. */ /* doesn't need to prepare for retry GO Nego REQ. */ /*pAd->P2pTable.Client[index].P2pClientState = P2PSTATE_DISCOVERY; */ /* Test Plan 5.1.22 */ P2pStopConnectThis(pAd); /* Reset Peer State as Default. */ pAd->P2pTable.Client[index].P2pClientState = P2PSTATE_DISCOVERY_CLIENT; /* Turn Back to My Listen Channel. */ P2pStopScan(pAd); /* Back to LISTEN State. */ MlmeEnqueue(pAd, P2P_DISC_STATE_MACHINE, P2P_DISC_CANL_CMD_EVT, 0, NULL, 0); DBGPRINT(RT_DEBUG_TRACE, (" P2P - = %s \n", decodeP2PState(pAd->P2pCfg.P2PConnectState))); } Channel = pAd->P2pCfg.ListenChannel; AsicSwitchChannel(pAd, Channel, FALSE); AsicLockChannel(pAd, Channel); return; } else if (StatusCode != 0) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Ignore Go Negociation Response Status Code not Success. = %d \n", StatusCode)); return; } /* Check Peer is in the valid state to receive Go Negociation Response. */ index = P2pGroupTabSearch(pAd, DevAddr); if (index == P2P_NOT_FOUND) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Ignore Go Negociation Response from Unknown device. \n")); return; } else { } if (index < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[index]; pP2pEntry->ConfigMethod = ConfigMethod; /* Check peer capability */ pP2pEntry->ListenChannel = Channel; if (OpChannel != 0) pP2pEntry->OpChannel = OpChannel; pP2pEntry->GoIntent = Intent; pP2pEntry->StateCount = 0; pP2pEntry->bValid = FALSE; pP2pEntry->Dpid = Dpid; if (!NdisEqualMemory(AllZero, IfAddr, MAC_ADDR_LEN)) RTMPMoveMemory(pP2pEntry->InterfaceAddr, IfAddr, MAC_ADDR_LEN); if (SsidLen > 0) { RTMPMoveMemory(pP2pEntry->Ssid, Ssid, SsidLen); pP2pEntry->SsidLen = SsidLen; } P2PPrintP2PEntry(pAd, index); /* If this peer is provistioned, dpid should follows spec's assignment on page 33 */ if (P2P_TEST_FLAG(pP2pEntry, P2PFLAG_PROVISIONED)) { DBGPRINT(RT_DEBUG_ERROR, ("P2P provisioned -dpid= %x. ConfigMethod = %s.\n", Dpid, decodeConfigMethod(pP2pEntry->ConfigMethod))); switch(pP2pEntry->ConfigMethod) { case WSC_CONFMET_DISPLAY: if ((Dpid != DEV_PASS_ID_REG) && (Dpid != DEV_PASS_ID_PIN)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P -1 Ignore Go Negociation Response with wrong dpid \n")); return; } break; case WSC_CONFMET_KEYPAD: if (Dpid != DEV_PASS_ID_USER) { DBGPRINT(RT_DEBUG_ERROR, (" P2P -3 Ignore Go Negociation Response with wrong dpid \n")); return; } break; default: break; } } P2pSetRule(pAd, index, IfAddr, TempIntent, OpChannel); if ((pP2PCtrl->P2PConnectState == P2P_ANY_IN_FORMATION_AS_GO) && (pAd->P2pTable.Client[index].P2pClientState != P2PSTATE_SENT_GO_NEG_REQ)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Existing peer stay in another state. = %d. \n", pAd->P2pTable.Client[index].P2pClientState)); return; } P2PSendGoNegoConfirm(pAd, pFrame->Token, index, pP2pEntry->addr); } } /* ========================================================================== Description: The routine that is called when receiving Go Negociation Request packet. Parameters: Note: ========================================================================== */ VOID P2pReceGoNegoReqAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; UCHAR DevType[8] = {0}, DevAddr[6] = {0}, BssidAddr[6] = {0}; UCHAR Channel, OpChannel, Intent, index, StatusCode; UCHAR GroupCap, DeviceCap, DeviceName[32], DeviceNameLen; ULONG FrameLen; ULONG TempLen; UCHAR SsidLen = 0; RT_P2P_CLIENT_ENTRY *pP2pEntry; NDIS_STATUS NStatus; PUCHAR pOutBuffer = NULL; PUCHAR pDest; /* make Go Nego Rsp Status as PASSED at first. What if UI/wpa_supplicant is involved, send GO Nego Resp wiht Status = 1, and inform Upper layer, Go Nego Req is coming! if all operation is from iwpriv instead of UI. we shall type iwpriv p2p0 set p2pConfirmByUI=1 to unlock the condition, it will make RspStatus could become P2PSTATUS_SUCCESS. */ UCHAR RspStatus = P2PSTATUS_PASSED; UCHAR TempIntent; /*PUCHAR pP2pIE; */ USHORT Dpid, SentDpid, ConfigMethod; BOOLEAN bSendEvent = FALSE; UCHAR AllZero[MAC_ADDR_LEN] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; BOOLEAN Cancelled; #ifdef WFD_SUPPORT ULONG WfdSubelementLen = 0; PUCHAR WfdSubelement = NULL; #endif /* WFD_SUPPORT */ DBGPRINT(RT_DEBUG_TRACE, ("GOGOGOGO P2P - receive Go Neg Request. MsgLen = %ld \n", Elem->MsgLen)); #ifdef WFD_SUPPORT os_alloc_mem(pAd, &WfdSubelement, MAX_VIE_LEN); if (WfdSubelement == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s::Allocate memory size(=%d) failed\n", __FUNCTION__, MAX_VIE_LEN)); return; } #endif /* WFD_SUPPORT */ RTMPZeroMemory(&DeviceName[0], 32); pP2pEntry = NULL; if (IS_P2P_DEVICE_DISCOVERING(pAd)) P2pStopScan(pAd); /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, &Dpid, &GroupCap, &DeviceCap, DeviceName, &DeviceNameLen, DevAddr, BssidAddr, NULL, &SsidLen, NULL, &ConfigMethod, NULL, DevType, &Channel, &OpChannel, NULL, &TempIntent, &StatusCode, NULL, #ifdef WFD_SUPPORT &WfdSubelementLen, WfdSubelement, #endif /* WFD_SUPPORT */ NULL); Intent = TempIntent >>1; DBGPRINT(RT_DEBUG_TRACE, (" DevAddr in P2P IE = %02x %02x %02x %02x %02x %02x \n", PRINT_MAC(DevAddr))); DBGPRINT(RT_DEBUG_TRACE, (" Addr2 = %02x %02x %02x %02x %02x %02x \n", PRINT_MAC(pFrame->p80211Header.Addr2))); DBGPRINT(RT_DEBUG_TRACE, (" its bssid = %02x %02x %02x %02x %02x %02x \n", PRINT_MAC(BssidAddr))); DBGPRINT(RT_DEBUG_TRACE, (" Dpid = %x ,%s \n", Dpid, decodeDpid(Dpid))); index = P2pGroupTabSearch(pAd, DevAddr); if ((index != P2P_NOT_FOUND) && (pP2PCtrl->bP2pReSendTimerRunning)) { pP2PCtrl->bP2pReSendTimerRunning = FALSE; pAd->P2pTable.Client[index].ReTransmitCnt = 0; RTMPCancelTimer(&pP2PCtrl->P2pReSendTimer, &Cancelled); } DBGPRINT(RT_DEBUG_TRACE, ("P2P -Peer[%d] Intent = %x . OpChannel = %x. My Intent = %x. TempIntent = %x\n", index, Intent, Channel, pP2PCtrl->GoIntentIdx, TempIntent)); if (index == P2P_NOT_FOUND) { DBGPRINT(RT_DEBUG_TRACE, ("P2P -1 insert\n")); index = P2pGroupTabInsert(pAd, DevAddr, P2PSTATE_DISCOVERY, NULL, 0, DeviceCap, GroupCap); if (index < MAX_P2P_GROUP_SIZE) { pP2PCtrl->PopUpIndex = index; RTMPMoveMemory(pAd->P2pTable.Client[index].DeviceName, DeviceName, 32); DBGPRINT(RT_DEBUG_ERROR, ("From a unknown peer. Pop up setting windows. %d\n", pP2PCtrl->PopUpIndex)); pAd->P2pTable.Client[index].DeviceNameLen = DeviceNameLen; pAd->P2pTable.Client[index].DeviceName[pAd->P2pTable.Client[index].DeviceNameLen] = 0x0; pP2pEntry = &pAd->P2pTable.Client[index]; pP2pEntry->ConfigMethod = ConfigMethod; } bSendEvent = TRUE; } if (index < MAX_P2P_GROUP_SIZE) { if ((pP2PCtrl->bConfirmByUI) || (pAd->P2pCfg.bSigmaEnabled == TRUE)) { RspStatus = P2PSTATUS_SUCCESS; } pP2pEntry = &pAd->P2pTable.Client[index]; pP2pEntry->ConfigMethod = ConfigMethod; if (!NdisEqualMemory(AllZero, BssidAddr, MAC_ADDR_LEN)) RTMPMoveMemory(pP2pEntry->InterfaceAddr, BssidAddr, MAC_ADDR_LEN); #ifdef WFD_SUPPORT if (WfdSubelementLen) { /*Note: In order to enhance the compatibility. We do not update the session_avail bit carried in GO-Nego-Req, as some devices will clear this bit during GO-Nego phase. So we backup its original value, and then set it back. */ UCHAR session_avail_backup=pP2pEntry->WfdEntryInfo.session_avail; WfdParseSubElmt(pAd, &pP2pEntry->WfdEntryInfo, (PVOID)WfdSubelement, WfdSubelementLen); pP2pEntry->WfdEntryInfo.session_avail=session_avail_backup; } #endif /* WFD_SUPPORT */ P2PPrintP2PEntry(pAd, index); DBGPRINT(RT_DEBUG_TRACE, ("My P2P Dpid = %x. State = %s. \n", pP2PCtrl->Dpid, decodeP2PClientState(pP2pEntry->P2pClientState))); if (IS_P2P_PEER_CLIENT_OP(pP2pEntry) || IS_P2P_PEER_WPAPSK(pP2pEntry) || IS_P2P_PEER_PROVISIONING(pP2pEntry)) { /* Receive Go Neg Req when this peer's state is operating or doing provisioning. Delete this peer. */ DBGPRINT(RT_DEBUG_TRACE, (" P2P - Existing peer state is %d. %s,. Delete it.\n", pP2pEntry->P2pClientState, decodeP2PClientState(pP2pEntry->P2pClientState))); goto CleanUp; } if (pP2pEntry->P2pClientState > P2PSTATE_GO_DONE) { DBGPRINT(RT_DEBUG_TRACE, (" P2P - Existing peer state is %d . %s,. return.\n", pP2pEntry->P2pClientState, decodeP2PClientState(pP2pEntry->P2pClientState))); goto CleanUp; } else if (pP2pEntry->P2pClientState == P2PSTATE_SENT_GO_NEG_REQ) { if (IsP2pFirstMacSmaller(pP2pEntry->addr, pP2PCtrl->CurrentAddress)) { DBGPRINT(RT_DEBUG_TRACE, (" P2P - Dual GO Req. Existing peer state is %s. \n", decodeP2PClientState(pP2pEntry->P2pClientState))); goto CleanUp; } } if (P2P_GO_ON(pAd)) { /* I am GO . Don't need go through GO NEgo process. So return fail. */ RspStatus = P2PSTATUS_INVALID_PARM; } else if (pP2PCtrl->Dpid == DEV_PASS_ID_NOSPEC) { /* Test Plan 5.1.21 */ RspStatus = P2PSTATUS_PASSED; } } else goto CleanUp; /* Ban Request from the same GoIntent index. */ if ((Intent == pP2PCtrl->GoIntentIdx) && (Intent == 15)) { RspStatus = P2PSTATUS_BOTH_INTENT15; DBGPRINT(RT_DEBUG_ERROR, (" P2pReceGoNegoReqAction -Receive a peer that has the same Go Intent index as mine. \n")); #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_GO_NEGO_FAIL_INTENT, NULL, DevAddr); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ } /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) { DBGPRINT(RT_DEBUG_ERROR, (" MlmeAllocateMemory fail - . \n")); goto CleanUp; } pDest = pOutBuffer; if (RspStatus == P2PSTATUS_SUCCESS) { pP2pEntry->P2pClientState = P2PSTATE_WAIT_GO_COMFIRM; pP2pEntry->StateCount = 0; pP2pEntry->bValid = FALSE; pP2pEntry->Dpid = Dpid; P2pSetRule(pAd, index, BssidAddr, TempIntent, OpChannel); /* If I am Go, Use my channel to set in the Go Nego Rsp. */ if (pP2pEntry->Rule == P2P_IS_CLIENT) OpChannel = pP2PCtrl->GroupChannel; /* Change beacon content */ pP2PCtrl->P2pCapability[1] |= (GRPCAP_GROUP_FORMING); P2pUpdateBssBeacon(pAd, pP2PCtrl->P2pCapability, NULL); } /*SentDpid = DEV_PASS_ID_PIN; */ if (P2P_TEST_FLAG(pP2pEntry, P2PFLAG_PROVISIONED)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Go Negociation Responce to provisioned \n")); if (pP2PCtrl->ConfigMethod == WSC_CONFMET_DISPLAY) SentDpid = DEV_PASS_ID_REG; else if (pP2PCtrl->ConfigMethod == WSC_CONFMET_LABEL) SentDpid = DEV_PASS_ID_PIN; else if (pP2PCtrl->ConfigMethod == WSC_CONFMET_KEYPAD) { if (pP2PCtrl->bSigmaEnabled == FALSE) RspStatus = P2PSTATUS_PASSED; SentDpid = DEV_PASS_ID_USER; } else if (pP2PCtrl->ConfigMethod == WSC_CONFMET_PBC) SentDpid = DEV_PASS_ID_PBC; } else { if (Dpid == DEV_PASS_ID_PIN) SentDpid = DEV_PASS_ID_USER; else if (Dpid == DEV_PASS_ID_USER) SentDpid = DEV_PASS_ID_PIN; else if (Dpid == DEV_PASS_ID_REG) { if (pP2PCtrl->bSigmaEnabled == FALSE) RspStatus = P2PSTATUS_PASSED; SentDpid = DEV_PASS_ID_USER; } else if (Dpid == DEV_PASS_ID_PBC) SentDpid = DEV_PASS_ID_PBC; else DBGPRINT(RT_DEBUG_ERROR, ("Peer Go Nego Req DPID = %x\n", Dpid)); } /* Save peer capability */ pP2pEntry->ListenChannel = Channel; pP2pEntry->OpChannel = OpChannel; #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT if (bSendEvent) P2pSendWirelessEvent(pAd, RT_P2P_DEVICE_FIND, &pAd->P2pTable.Client[index], pFrame->p80211Header.Addr2); P2pSendWirelessEvent(pAd, RT_P2P_RECV_GO_NEGO_REQ, &pAd->P2pTable.Client[index], pFrame->p80211Header.Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ /*P2P_SetWscRule(pAd, SentDpid); */ SentDpid = pP2PCtrl->Dpid; DBGPRINT(RT_DEBUG_TRACE, (" P2P - ReceDpid = %x. SentDpid %x \n", (Dpid), (SentDpid))); DBGPRINT(RT_DEBUG_TRACE, (" P2P - ReceDpid = %s. SentDpid %s \n", decodeDpid(Dpid), decodeDpid(SentDpid))); P2PMakeGoNegoRsp(pAd, DevAddr, SentDpid, pFrame->Token, TempIntent, OpChannel, RspStatus, pOutBuffer, &TempLen); FrameLen = TempLen; pDest += TempLen; /* : copy req to rsp first. */ DBGPRINT(RT_DEBUG_TRACE, (" P2P - Make Go Negociation Responce Length = %ld.RspStatus = %d \n", FrameLen, RspStatus)); MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen); MlmeFreeMemory(pAd, pOutBuffer); if (RspStatus == P2PSTATUS_PASSED) { P2P_CMD_STRUCT P2pCmd; pP2pEntry->Dpid = Dpid; P2pSetRule(pAd, index, BssidAddr, TempIntent, OpChannel); /*UCHAR ClientState = pAd->P2pTable.Client[index].P2pClientState;*/ COPY_MAC_ADDR(&P2pCmd.Addr[0], pAd->P2pTable.Client[index].addr); P2pCmd.Idx = index; //if (pP2PCtrl->ConfigMethod == WSC_CONFMET_DISPLAY) if (pP2PCtrl->ConfigMethod != WSC_CONFMET_KEYPAD) MlmeEnqueue(pAd, P2P_GO_FORM_STATE_MACHINE, P2P_SEND_PASSED_CMD_EVT, sizeof(P2P_CMD_STRUCT), &P2pCmd, 0); #ifdef HE_BD_SUPPORT if (pP2PCtrl->ConfigMethod == WSC_CONFMET_KEYPAD) { pP2PCtrl->bConfirmByUI = false; // If Keypad, still need UI confirm to run connection sequence. } #endif } /*P2PPrintP2PEntry(pAd, index); */ /* else ignore this Go Negociation Request because P2P table reach maximum. */ CleanUp: #ifdef WFD_SUPPORT if (WfdSubelement) os_free_mem(pAd, WfdSubelement); #endif /* WFD_SUPPORT */ return; } /* ========================================================================== Description: The routine that is called when receiving Client Discovery Request Public Action Frame packet. Parameters: Note: ========================================================================== */ UCHAR DevDiscCnt = 0; VOID P2pReceDevDisReqAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; UCHAR devAddr[6], BssidAddr[6], OpChannel, StatusCode; UCHAR GroupCap, DeviceCap, DeviceNameLen; UCHAR SsidLen; UCHAR p2pClientIndex; RT_P2P_CLIENT_ENTRY *pEntry = NULL; MAC_TABLE_ENTRY *pMacEntry = NULL; ULONG TotalFrameLen; DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2pReceDevDisReqAction = %ld\n", Elem->MsgLen)); /* stop scan and lock at Listen Channel. */ if ((!P2P_GO_ON(pAd)) && (!P2P_CLI_ON(pAd))) { P2pStopScan(pAd); AsicSwitchChannel(pAd, pAd->P2pCfg.ListenChannel, FALSE); AsicLockChannel(pAd, pAd->P2pCfg.ListenChannel); } /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, NULL, &GroupCap, &DeviceCap, NULL, &DeviceNameLen, devAddr, NULL, BssidAddr, &SsidLen, NULL, NULL, NULL, NULL, NULL, &OpChannel, NULL, NULL, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); /* Use TX to decide who is sending. */ p2pClientIndex = P2pGroupTabSearch(pAd, devAddr); if (p2pClientIndex < MAX_P2P_GROUP_SIZE) { pEntry = &pAd->P2pTable.Client[p2pClientIndex]; pMacEntry = MacTableLookup(pAd, pEntry->InterfaceAddr); } DBGPRINT(RT_DEBUG_ERROR, ("P2P - ask for p2p client[%d] = %02x:%02x:%02x:%02x:%02x:%02x.\n", p2pClientIndex, PRINT_MAC(devAddr))); if (((pMacEntry != NULL) && (pEntry != NULL) && (pEntry->P2pClientState == P2PSTATE_CLIENT_WPS_DONE))) { MLME_P2P_ACTION_STRUCT P2PActReq; NdisZeroMemory(&P2PActReq, sizeof(P2PActReq)); DBGPRINT(RT_DEBUG_TRACE, ("to interface Addr = %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(pEntry->InterfaceAddr))); COPY_MAC_ADDR(P2PActReq.Addr, pEntry->InterfaceAddr); P2PActReq.TabIndex = p2pClientIndex; MlmeEnqueue(pAd, P2P_ACTION_STATE_MACHINE, MT2_MLME_P2P_GO_DIS_REQ, sizeof(MLME_P2P_ACTION_STRUCT), (PVOID)&P2PActReq, 0); MlmeHandler(pAd); DevDiscCnt++; /* Device Discvoery Response is delayed until I get the GO Discovery Request Frame's Ack. */ /* Assume always success. Send back response. */ RTMPMoveMemory(&pAd->P2pCfg.LatestP2pPublicFrame, pFrame, sizeof(P2P_SAVED_PUBLIC_FRAME)); OS_WAIT(500); P2PSendDevDisRsp(pAd, P2PSTATUS_SUCCESS, pAd->P2pCfg.LatestP2pPublicFrame.Token, pAd->P2pCfg.LatestP2pPublicFrame.p80211Header.Addr2, &TotalFrameLen); } else { DBGPRINT(RT_DEBUG_TRACE, ("%s:: P2P -can't find p2p client .Status fail\n", __FUNCTION__)); P2PSendDevDisRsp(pAd, P2PSTATUS_IMCOMPA_PARM, pFrame->Token, pFrame->p80211Header.Addr2, &TotalFrameLen); } } /* ========================================================================== Description: The routine that is called when receiving Client Discovery Response Public Action Frame packet. Parameters: Note: ========================================================================== */ VOID P2pReceDevDisRspAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; UCHAR DevType[8], DevAddr[6], BssidAddr[6], Channel, OpChannel, index, StatusCode; UCHAR GroupCap, DeviceCap, DeviceNameLen, TempIntent; UCHAR SsidLen = 0; RT_P2P_CLIENT_ENTRY *pP2pEntry; USHORT Dpid, ConfigMethod; UCHAR ClientP2PIndex = P2P_NOT_FOUND, i; DBGPRINT(RT_DEBUG_ERROR, ("GOGO P2P - P2pReceDevDisRspAction MsgLen = %ld ", Elem->MsgLen)); pP2pEntry = NULL; index = P2pGroupTabSearch(pAd, pFrame->p80211Header.Addr2); if (index == P2P_NOT_FOUND) return; pP2pEntry = &pAd->P2pTable.Client[index]; DBGPRINT(RT_DEBUG_ERROR, ("P2pReceDevDisRspAction %s.\n ", decodeP2PClientState(pP2pEntry->P2pClientState))); if (pP2pEntry->P2pClientState != P2PSTATE_GO_DISCO_COMMAND) return; /* Change State back to P2PSTATE_DISCOVERY_GO */ /* Because P2PSTATE_GO_DISCO_COMMAND is from P2PSTATE_DISCOVERY_GO */ pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY_GO; /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, &Dpid, &GroupCap, &DeviceCap, NULL, &DeviceNameLen, DevAddr, BssidAddr, NULL, &SsidLen, NULL, &ConfigMethod, NULL, DevType, &Channel, &OpChannel, NULL, &TempIntent, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); if (StatusCode == P2PSTATUS_SUCCESS) { for (i = index; i < MAX_P2P_GROUP_SIZE;i++) { if (pAd->P2pTable.Client[i].P2pClientState == P2PSTATE_CLIENT_DISCO_COMMAND) ClientP2PIndex = i; } if (ClientP2PIndex == P2P_NOT_FOUND) return; pAd->P2pTable.Client[ClientP2PIndex].P2pClientState = P2PSTATE_CONNECT_COMMAND; P2pStartGroupForm(pAd, pAd->P2pTable.Client[ClientP2PIndex].addr, ClientP2PIndex); } } /* ========================================================================== Description: The routine that is called when receiving Invitation Request Action Frame packet. Parameters: Note: ========================================================================== */ VOID P2pReceInviteReqAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; UCHAR DevType[8], Ssid[32], DevAddr[6], BssidAddr[6], InfAddr[6], OpChannel, index, StatusCode; UCHAR GroupCap, DeviceCap, DeviceNameLen; ULONG FrameLen; UCHAR ChannelList[MAX_NUM_OF_CHANNELS]; UCHAR SsidLen, p2pindex; /*RT_P2P_CLIENT_ENTRY *pP2pEntry;*/ UCHAR RspStatus = P2PSTATUS_INVALID_PARM; UCHAR MyRule = P2P_IS_GO; USHORT ConfigMethod; UCHAR InviteFlag; BOOLEAN bReinvoke = FALSE; INT P2PEntryCnt; RTMPZeroMemory(ChannelList, MAX_NUM_OF_CHANNELS); RTMPZeroMemory(Ssid, MAX_LEN_OF_SSID); /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, NULL, &GroupCap, &DeviceCap, NULL, &DeviceNameLen, DevAddr, InfAddr, BssidAddr, &SsidLen, Ssid, &ConfigMethod, NULL, DevType, NULL, &OpChannel, ChannelList, NULL, &StatusCode, &InviteFlag, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); DBGPRINT(RT_DEBUG_ERROR, ("P2pReceInviteReqAction ==>InviteFlag = %d. %s \n", InviteFlag, decodeP2PState(pP2PCtrl->P2PConnectState))); /* Use TX to decide who is sending. */ RTMPMoveMemory(DevAddr, pFrame->p80211Header.Addr2, MAC_ADDR_LEN); DBGPRINT(RT_DEBUG_ERROR, ("OpChannel = %d. \n", OpChannel)); DBGPRINT(RT_DEBUG_ERROR, ("BssidAddr = %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(BssidAddr))); DBGPRINT(RT_DEBUG_ERROR, ("DevAddr = %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(DevAddr))); DBGPRINT(RT_DEBUG_ERROR, ("Ssid[%d] = %s. \n", SsidLen, Ssid)); p2pindex = P2pGroupTabSearch(pAd, DevAddr); { p2pindex = P2pGroupTabInsert(pAd, DevAddr, P2PSTATE_DISCOVERY_GO, Ssid, SsidLen, DeviceCap, GroupCap); } index = P2pPerstTabSearch(pAd, DevAddr, BssidAddr, InfAddr); DBGPRINT(RT_DEBUG_ERROR, ("perst index = %d. p2pindex = %d. \n", index, p2pindex)); DBGPRINT(RT_DEBUG_ERROR, ("My Dpid = %d. Dpid = %s.\n", pP2PCtrl->Dpid, decodeDpid(pP2PCtrl->Dpid))); P2PPrintP2PEntry(pAd, p2pindex); if (p2pindex == P2P_NOT_FOUND) return; if (P2P_GO_ON(pAd)) { P2PEntryCnt = P2PGetEntryCnt(pAd); if ( (pP2PCtrl->p2pMaxEntry!= 0 ) && (P2PEntryCnt >= pP2PCtrl->p2pMaxEntry) ) { DBGPRINT(RT_DEBUG_ERROR, ("P2pReceInviteReqAction ==>P2PGetEntryCnt = %d\n", P2PEntryCnt)); RspStatus = P2PSTATUS_UNABLE; P2PMakeInviteRsp(pAd, MyRule, pFrame->Token, pFrame->p80211Header.Addr2, pP2PCtrl->CurrentAddress, &OpChannel, &RspStatus, &FrameLen); return; } } if ((index < MAX_P2P_TABLE_SIZE) && ((InviteFlag & P2P_INVITE_FLAG_REINVOKE) == P2P_INVITE_FLAG_REINVOKE)) bReinvoke = TRUE; /* case 1: Reinvoke case: */ /* If I have credential and both enable persistent. */ if (bReinvoke == TRUE) { DBGPRINT(RT_DEBUG_ERROR, ("Invite: reinvoke. \n")); P2pCheckInviteReq(pAd, (pAd->P2pTable.PerstEntry[index].MyRule == P2P_IS_GO), index, ChannelList, BssidAddr, OpChannel, Ssid, SsidLen, &RspStatus); if (RspStatus == P2PSTATUS_SUCCESS) { /* Find peer,. So set Status = success. */ if (pAd->P2pTable.PerstEntry[index].MyRule == P2P_IS_GO) { OpChannel = pP2PCtrl->GroupChannel; pP2PCtrl->GroupOpChannel = OpChannel; /* pAd->StaCfg.WscControl.WscAPChannel = OpChannel; */ DBGPRINT(RT_DEBUG_ERROR, ("Invite: Decide to use OpChannel = %d for group \n", OpChannel)); p2pindex = P2pGroupTabInsert(pAd, DevAddr, P2PSTATE_CLIENT_WPS, Ssid, SsidLen, DeviceCap, GroupCap); if (p2pindex < MAX_P2P_GROUP_SIZE) { pAd->P2pTable.Client[p2pindex].P2pClientState = P2PSTATE_WAIT_REVOKEINVITE_RSP_ACK; } else { DBGPRINT(RT_DEBUG_ERROR, ("Invite: reach my limit. Send back RspStatus = %d. \n", RspStatus)); RspStatus = P2PSTATUS_LIMIT; } } else { MyRule = P2P_IS_CLIENT; pP2PCtrl->GroupChannel = OpChannel; pP2PCtrl->GroupOpChannel = OpChannel; /*P2pCopyPerstParmToCfg(pAd, index); */ DBGPRINT(RT_DEBUG_ERROR, ("Invite: Reinvoke as client for group \n")); pP2PCtrl->P2PConnectState = P2P_ANY_IN_FORMATION_AS_CLIENT; /* Update the Client state and SSID. */ P2pGroupTabInsert(pAd, DevAddr, P2PSTATE_GO_WPS, Ssid, SsidLen, DeviceCap, GroupCap); /*P2pGoNegoDone(pAd, pAd->PortList[pAd->P2pCfg.PortNumber], &pAd->P2pTable.Client[index]); */ pP2PCtrl->P2PConnectState = P2P_DO_WPS_ENROLLEE; /*P2pWpsDone(pAd, DevAddr); */ COPY_MAC_ADDR(pAd->ApCfg.ApCliTab[0].CfgApCliBssid, pP2PCtrl->Bssid); } } } /* case 2: abNormal invitation cases:... */ else if ((InviteFlag & P2P_INVITE_FLAG_REINVOKE) == P2P_INVITE_FLAG_REINVOKE) { /* Peer ask me to reinvoke. But I can't find my record. Send back error code. */ RspStatus = P2PSTATUS_UNKNOWN_GROUP; /* In this case, we need to do P2P GO formation with that P2P peer again. Therefore we need to update P2pClientState and Rule here for preventing wrong action in P2pReceProvisionReqAction. */ pAd->P2pTable.Client[p2pindex].Rule = P2P_IS_DEVICE; pAd->P2pTable.Client[p2pindex].P2pClientState = P2PSTATE_DISCOVERY; DBGPRINT(RT_DEBUG_ERROR, ("Invite: Can't find Credential. \n")); } /* case 3: Normal invitation cases: */ else { if (RTMPEqualMemory(BssidAddr, ZERO_MAC_ADDR, MAC_ADDR_LEN)) { DBGPRINT(RT_DEBUG_ERROR, ("Invite Req carry allzero Bssid, \n")); } else if (IS_P2P_CONNECT_IDLE(pAd) || (pAd->P2pCfg.P2PConnectState == P2P_INVITE)) { P2pCheckInviteReqFromExisting(pAd, ChannelList, DevAddr, OpChannel, Ssid, SsidLen, &RspStatus); DBGPRINT(RT_DEBUG_ERROR, ("Invite: Got invitation from P2P Client . RspStatus = %d \n", RspStatus)); if (RspStatus == P2PSTATUS_SUCCESS) { RTMPZeroMemory(pP2PCtrl->SSID, MAX_LEN_OF_SSID); RTMPMoveMemory(pP2PCtrl->SSID, Ssid, SsidLen); RTMPZeroMemory(pAd->P2pTable.Client[p2pindex].Ssid, MAX_LEN_OF_SSID); RTMPMoveMemory(pAd->P2pTable.Client[p2pindex].Ssid, Ssid, SsidLen); pAd->P2pTable.Client[p2pindex].SsidLen = SsidLen; COPY_MAC_ADDR(pP2PCtrl->Bssid, BssidAddr); COPY_MAC_ADDR(pAd->P2pTable.Client[p2pindex].bssid, BssidAddr); COPY_MAC_ADDR(pAd->P2pTable.Client[p2pindex].InterfaceAddr, BssidAddr); pP2PCtrl->SSIDLen = SsidLen; pP2PCtrl->GroupOpChannel = OpChannel; pAd->P2pTable.Client[p2pindex].OpChannel = OpChannel; pAd->P2pTable.Client[p2pindex].ConfigMethod = ConfigMethod; MyRule = P2P_IS_CLIENT; } } } #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_RECV_INVITE_REQ, &pAd->P2pTable.Client[p2pindex], pFrame->p80211Header.Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ DBGPRINT(RT_DEBUG_ERROR, ("P2p Send Invite Rsp RspStatus = %d. \n", RspStatus)); P2PMakeInviteRsp(pAd, MyRule, pFrame->Token, pFrame->p80211Header.Addr2, pP2PCtrl->CurrentAddress, &OpChannel, &RspStatus, &FrameLen); /* Start Provision */ if (((bReinvoke == FALSE) && (MyRule == P2P_IS_CLIENT)) && ((RspStatus == P2PSTATUS_SUCCESS) || ((RspStatus == P2PSTATUS_UNKNOWN_GROUP)))) { P2pConnectPrepare(pAd, DevAddr, P2PSTATE_PROVISION_COMMAND); } else if ((bReinvoke == TRUE) && (RspStatus == P2PSTATUS_SUCCESS)) { PWSC_CTRL pWscControl; if (MyRule == P2P_IS_CLIENT) { POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie; pObj->ioctl_if_type = INT_P2P; pWscControl = &pAd->ApCfg.ApCliTab[0].WscControl; /* P2P CLIENT up. */ Set_P2p_OpMode_Proc(pAd, "2"); OS_WAIT(500); pObj->ioctl_if_type = INT_APCLI; NdisMoveMemory(pP2PCtrl->Bssid, pAd->P2pTable.PerstEntry[index].Addr, MAC_ADDR_LEN); WscWriteConfToApCliCfg(pAd, pWscControl, &pAd->P2pTable.PerstEntry[index].Profile, TRUE); pObj->ioctl_if_type = INT_P2P; if ((pAd->CommonCfg.Channel != OpChannel) && (!INFRA_ON(pAd))) { pAd->CommonCfg.Channel = OpChannel; AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE); AsicLockChannel(pAd, pAd->CommonCfg.Channel); } /* P2P AP-Client Enable. */ Set_P2pCli_Enable_Proc(pAd, "1"); } else { if (!P2P_GO_ON(pAd) && !P2P_CLI_ON(pAd)) { pWscControl = &pAd->ApCfg.MBSSID[0].WscControl; WscWriteConfToPortCfg(pAd, pWscControl, &pAd->P2pTable.PerstEntry[index].Profile, TRUE); P2P_GoStop(pAd); P2P_GoStartUp(pAd, MAIN_MBSSID); } } } } /* ========================================================================== Description: The routine that is called when receiving Invitation Response Action Frame packet. Parameters: Note: ========================================================================== */ VOID P2pReceInviteRspAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; UCHAR DevType[8], devAddr[6], BssidAddr[6] = {0}, OpChannel, index, StatusCode; UCHAR GroupCap, DeviceCap, DeviceNameLen; USHORT ConfigMethod; UCHAR SsidLen, ChannelList[MAX_NUM_OF_CHANNELS]; PRT_P2P_CLIENT_ENTRY pP2pEntry = NULL; PWSC_CTRL pWscControl; RTMPZeroMemory(ChannelList, MAX_NUM_OF_CHANNELS); RTMPZeroMemory(BssidAddr, MAC_ADDR_LEN); /* Get Request content capability */ P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, NULL, &GroupCap, &DeviceCap, NULL, &DeviceNameLen, devAddr, NULL, BssidAddr, &SsidLen, NULL, &ConfigMethod, NULL, DevType, NULL, &OpChannel, ChannelList, NULL, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); DBGPRINT(RT_DEBUG_TRACE, ("P2pReceInviteRspAction ==> \n")); DBGPRINT(RT_DEBUG_TRACE, ("TA = %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(pFrame->p80211Header.Addr2))); if ((StatusCode == P2PSTATUS_SUCCESS) || (StatusCode == P2PSTATUS_PASSED)) { index = P2pPerstTabSearch(pAd, pFrame->p80211Header.Addr2, NULL, NULL); if ((index < MAX_P2P_TABLE_SIZE) && (IS_PERSISTENT_ON(pAd))) { /* this is a persistent connection. */ pP2PCtrl->ExtListenInterval = P2P_EXT_LISTEN_INTERVAL; pP2PCtrl->ExtListenPeriod = P2P_EXT_LISTEN_PERIOD; /* Find peer,. So set Status = success. */ if (pAd->P2pTable.PerstEntry[index].MyRule == P2P_IS_GO) { OpChannel = pP2PCtrl->GroupChannel; pP2PCtrl->GroupOpChannel = OpChannel; P2pCopyPerstParmToCfg(pAd, index); DBGPRINT(RT_DEBUG_TRACE, ("Decide to use OpChannel = %d for group \n", OpChannel)); P2pGroupTabInsert(pAd, pFrame->p80211Header.Addr2, P2PSTATE_CLIENT_WPS, NULL, SsidLen, 0, 0); pP2pEntry = &pAd->P2pTable.Client[index]; pWscControl = &pAd->ApCfg.MBSSID[0].WscControl; WscWriteConfToPortCfg(pAd, pWscControl, &pAd->P2pTable.PerstEntry[index].Profile, TRUE); P2P_GoStop(pAd); P2P_GoStartUp(pAd, MAIN_MBSSID); } else { POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie; pObj->ioctl_if_type = INT_P2P; pWscControl = &pAd->ApCfg.ApCliTab[0].WscControl; pP2PCtrl->GroupChannel = OpChannel; pP2PCtrl->GroupOpChannel = OpChannel; DBGPRINT(RT_DEBUG_ERROR, ("Reinvoke as client for group. OpChannel= %d\n", OpChannel)); pP2PCtrl->P2PConnectState = P2P_DO_WPS_ENROLLEE; /* Add some delay to connect GO */ P2pGroupTabInsert(pAd, pFrame->p80211Header.Addr2, P2PSTATE_REINVOKEINVITE_TILLCONFIGTIME, NULL, SsidLen, 0, 0); /* P2P CLIENT up. */ Set_P2p_OpMode_Proc(pAd, "2"); OS_WAIT(500); pObj->ioctl_if_type = INT_APCLI; WscWriteConfToApCliCfg(pAd, pWscControl, &pAd->P2pTable.PerstEntry[index].Profile, TRUE); pObj->ioctl_if_type = INT_P2P; /* P2P AP-Client Enable. */ Set_P2pCli_Enable_Proc(pAd, "1"); } } else { index = P2pGroupTabSearch(pAd, pFrame->p80211Header.Addr2); if (index < MAX_P2P_GROUP_SIZE) { if (P2P_GO_ON(pAd)) { pP2pEntry = &pAd->P2pTable.Client[index]; pP2pEntry->P2pClientState = P2PSTATE_GOT_GO_COMFIRM; P2pSetWps(pAd, pP2pEntry); DBGPRINT(RT_DEBUG_ERROR, ("P2pReceInviteRspAction Success. I am GO. \n")); } else if (P2P_CLI_ON(pAd)) DBGPRINT(RT_DEBUG_ERROR, ("P2pReceInviteRspAction Success. I am client. \n")); } } } } /* ========================================================================== Description: The routine that is called when receiving provistion request Action Frame packet. the Element field contains a single WPS IE. Parameters: Note: ========================================================================== */ VOID P2pReceProvisionReqAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PRT_P2P_CLIENT_ENTRY pP2pEntry = NULL; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; USHORT WpsConfigMethod; ULONG FrameLen; UCHAR p2pindex; UCHAR GroupCap, DeviceCap, DevAddr[MAC_ADDR_LEN], BssidAddr[MAC_ADDR_LEN], OpChannel; UCHAR Ssid[32], SsidLen; UCHAR DeviceName[32], DeviceNameLen; UCHAR DevType[8]; USHORT PeerWscMethod; BOOLEAN bSendEvent = FALSE; INT P2PEntryCnt; #ifdef WFD_SUPPORT ULONG WfdSubelementLen = 0; PUCHAR WfdSubelement = NULL; #endif /* WFD_SUPPORT */ DBGPRINT(RT_DEBUG_TRACE, ("P2pRece Provision ReqAction ==> \n")); #ifdef WFD_SUPPORT os_alloc_mem(pAd, &WfdSubelement, MAX_VIE_LEN); if (WfdSubelement == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s::Allocate memory size(=%d) failed\n", __FUNCTION__, MAX_VIE_LEN)); return; } #endif /* WFD_SUPPORT */ if (IS_P2P_DEVICE_DISCOVERING(pAd)) P2pStopScan(pAd); RTMPZeroMemory(&DeviceName, 32); P2pParseSubElmt(pAd, &pFrame->ElementID, (Elem->MsgLen - LENGTH_802_11 - 8), FALSE, NULL, &GroupCap, &DeviceCap, DeviceName, &DeviceNameLen, DevAddr, NULL, BssidAddr, &SsidLen, Ssid, NULL, &WpsConfigMethod, DevType, NULL, &OpChannel, NULL, NULL, NULL, NULL, #ifdef WFD_SUPPORT &WfdSubelementLen, WfdSubelement, #endif /* WFD_SUPPORT */ NULL); if (P2P_GO_ON(pAd)) { P2PEntryCnt = P2PGetEntryCnt(pAd); if ( (pP2PCtrl->p2pMaxEntry!= 0 ) && (P2PEntryCnt >= pP2PCtrl->p2pMaxEntry) ) { DBGPRINT(RT_DEBUG_ERROR, ("P2pRece Provision ReqAction ==>P2PGetEntryCnt = %d\n", P2PEntryCnt)); P2PSendProvisionRsp(pAd, 0, pFrame->Token, pFrame->p80211Header.Addr2, &FrameLen); return; } } DBGPRINT(RT_DEBUG_ERROR, ("P2pRece Provision ReqAction ==>ConfigMethod = %x %s\n", WpsConfigMethod, decodeConfigMethod(WpsConfigMethod))); /* Patch for other vendor that might send multiple config... IN spec, the config method in Provision Discovery req should carry config method that indicate SINGLE method. */ if (WpsConfigMethod == WSC_CONFMET_PBC && (pP2PCtrl->Dpid == DEV_PASS_ID_PBC)) { /* Correct. Keep going. */ } else if (WpsConfigMethod == WSC_CONFMET_DISPLAY && (pP2PCtrl->Dpid == DEV_PASS_ID_REG)) { /* Correct. Keep going. */ } else if (WpsConfigMethod == WSC_CONFMET_KEYPAD && (pP2PCtrl->Dpid == DEV_PASS_ID_USER)) { /* Correct. Keep going. */ } else if (WpsConfigMethod == WSC_CONFMET_LABEL && (pP2PCtrl->Dpid == DEV_PASS_ID_USER)) { /* Correct. Keep going. */ } else { if (WpsConfigMethod == WSC_CONFMET_PBC) { pP2PCtrl->WscMode = WSC_PBC_MODE; pP2PCtrl->Dpid = DEV_PASS_ID_PBC; pP2PCtrl->ConfigMethod = WSC_CONFMET_PBC; } else if (WpsConfigMethod == WSC_CONFMET_DISPLAY) { pP2PCtrl->WscMode = WSC_PIN_MODE; pP2PCtrl->Dpid = DEV_PASS_ID_REG; pP2PCtrl->ConfigMethod = WSC_CONFMET_DISPLAY; } else if (WpsConfigMethod == WSC_CONFMET_KEYPAD) { pP2PCtrl->WscMode = WSC_PIN_MODE; pP2PCtrl->Dpid = DEV_PASS_ID_USER; pP2PCtrl->ConfigMethod = WSC_CONFMET_KEYPAD; } else { DBGPRINT(RT_DEBUG_ERROR, ("P2pRece Provision ReqAction ==> Ignore this provision request here\n")); return; } } /* I only support those 4 method. */ if ( (WpsConfigMethod == WSC_CONFMET_DISPLAY) || (WpsConfigMethod == WSC_CONFMET_PBC) || (WpsConfigMethod == WSC_CONFMET_KEYPAD) || (WpsConfigMethod == 0x188)) { p2pindex = P2pGroupTabSearch(pAd, pFrame->p80211Header.Addr2); if (p2pindex >= MAX_P2P_GROUP_SIZE) { if ((GroupCap & GRPCAP_OWNER) == GRPCAP_OWNER) { p2pindex = P2pGroupTabInsert(pAd, pFrame->p80211Header.Addr2, P2PSTATE_DISCOVERY_GO, NULL, 0, DeviceCap, GroupCap); RTMPZeroMemory(pP2PCtrl->SSID, MAX_LEN_OF_SSID); RTMPMoveMemory(pP2PCtrl->SSID, Ssid, SsidLen); RTMPZeroMemory(pAd->P2pTable.Client[p2pindex].Ssid, MAX_LEN_OF_SSID); RTMPMoveMemory(pAd->P2pTable.Client[p2pindex].Ssid, Ssid, SsidLen); COPY_MAC_ADDR(pP2PCtrl->Bssid, BssidAddr); COPY_MAC_ADDR(pAd->P2pTable.Client[p2pindex].bssid, DevAddr); COPY_MAC_ADDR(pAd->P2pTable.Client[p2pindex].InterfaceAddr, DevAddr); pAd->P2pTable.Client[p2pindex].SsidLen = SsidLen; pP2PCtrl->SSIDLen = SsidLen; } else p2pindex = P2pGroupTabInsert(pAd, pFrame->p80211Header.Addr2, P2PSTATE_DISCOVERY, NULL, 0, DeviceCap, GroupCap); bSendEvent = TRUE; } if (p2pindex < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[p2pindex]; if (DeviceNameLen != 0) { RTMPMoveMemory(&pAd->P2pTable.Client[p2pindex].DeviceName[0], DeviceName, 32); pAd->P2pTable.Client[p2pindex].DeviceNameLen = DeviceNameLen; pAd->P2pTable.Client[p2pindex].DeviceName[pAd->P2pTable.Client[p2pindex].DeviceNameLen] = 0x0; } DBGPRINT(RT_DEBUG_TRACE, ("Update My Config Method to %s (%d)\n", decodeConfigMethod(pP2PCtrl->ConfigMethod ), pP2PCtrl->ConfigMethod)); if (WpsConfigMethod == WSC_CONFMET_KEYPAD) { pP2pEntry->ConfigMethod = WSC_CONFMET_DISPLAY; pP2pEntry->Dpid = DEV_PASS_ID_REG; } else if (WpsConfigMethod == WSC_CONFMET_PBC) { pP2pEntry->ConfigMethod = WSC_CONFMET_PBC; pP2pEntry->Dpid = DEV_PASS_ID_PBC; } else { pP2pEntry->ConfigMethod = WSC_CONFMET_KEYPAD; pP2pEntry->Dpid = DEV_PASS_ID_USER; } if ( pP2PCtrl->bProvAutoRsp == TRUE) { #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_RECV_PROV_REQ, &pAd->P2pTable.Client[p2pindex], pFrame->p80211Header.Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ /* Update My WPS Configuration. */ P2P_SetWscRule(pAd, p2pindex, &PeerWscMethod); /* Update Sigma.ConfigMethod after finished provision procedure. */ P2P_SET_FLAG(&pAd->P2pTable.Client[p2pindex], P2PFLAG_PROVISIONED); P2PSendProvisionRsp(pAd, WpsConfigMethod, pFrame->Token, pFrame->p80211Header.Addr2, &FrameLen); if ((P2P_GO_ON(pAd)) ) P2pSetWps(pAd, pP2pEntry); else if ((pP2pEntry->Rule == P2P_IS_GO) ) { pP2PCtrl->P2PConnectState = P2P_ANY_IN_FORMATION_AS_CLIENT; pP2pEntry->P2pClientState = P2PSTATE_GO_WPS; RTMPMoveMemory(pP2PCtrl->Bssid, pAd->P2pTable.Client[p2pindex].bssid, MAC_ADDR_LEN); RTMPMoveMemory(pAd->P2pCfg.Bssid, pP2pEntry->bssid, MAC_ADDR_LEN); RTMPZeroMemory(pP2PCtrl->SSID, 32); RTMPMoveMemory(pP2PCtrl->SSID, pP2pEntry->Ssid, pP2pEntry->SsidLen); pP2PCtrl->SSIDLen = pP2pEntry->SsidLen; pP2PCtrl->GroupOpChannel = pP2pEntry->OpChannel; P2pGoNegoDone(pAd, pP2pEntry); } else { pP2pEntry->P2pClientState = P2PSTATE_SENT_PROVISION_REQ; COPY_MAC_ADDR(pAd->P2pCfg.ConnectingMAC, pP2pEntry->addr); } DBGPRINT(RT_DEBUG_TRACE, ("Accept Provision Req ==>ConfigMethod = %s \n", decodeConfigMethod(WpsConfigMethod))); } else { if ( pP2PCtrl->P2pProvIndex== P2P_NOT_FOUND ) { pP2PCtrl->P2pProvConfigMethod = WpsConfigMethod; pP2PCtrl->P2pProvToken = pFrame->Token; RTMPMoveMemory(pP2PCtrl->ConnectingMAC, pFrame->p80211Header.Addr2, MAC_ADDR_LEN); pP2PCtrl->P2pCounter.UserAccept = 300; pP2PCtrl->P2pProvIndex = p2pindex; pP2PCtrl->P2pProvUserNotify = FALSE; #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_RECV_PROV_REQ, &pAd->P2pTable.Client[p2pindex], pFrame->p80211Header.Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ } else { pP2pEntry->P2pClientState = P2PSTATE_SENT_PROVISION_REQ; COPY_MAC_ADDR(pAd->P2pCfg.ConnectingMAC, pP2pEntry->addr); } } #ifdef WFD_SUPPORT if (WfdSubelementLen > 0) { WfdParseSubElmt(pAd, &pP2pEntry->WfdEntryInfo, (PVOID)WfdSubelement, WfdSubelementLen); } #endif /* WFD_SUPPORT */ } } else /* send null to indicate failure */ P2PSendProvisionRsp(pAd, 0, pFrame->Token, pFrame->p80211Header.Addr2, &FrameLen); #ifdef WFD_SUPPORT if (WfdSubelement) os_free_mem(pAd, WfdSubelement); #endif /* WFD_SUPPORT */ } /* ========================================================================== Description: The routine that is called when receiving provistion request Action Frame packet. Parameters: Note: ========================================================================== */ VOID P2pReceProvisionRspAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PRT_P2P_CLIENT_ENTRY pP2pEntry; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)Elem->Msg; USHORT ConfigMethod = 0, PeerWscMethod; /*ULONG FrameLen; */ UCHAR p2pindex = 0xff; /*PEID_STRUCT pEid;*/ PUCHAR Ptr = (PUCHAR) pFrame; PUCHAR pAddr = NULL; BOOLEAN Cancelled; p2pindex = P2pGroupTabSearch(pAd, pFrame->p80211Header.Addr2); if (p2pindex >= MAX_P2P_GROUP_SIZE) { DBGPRINT(RT_DEBUG_ERROR, ("P2pReceProvisionRspAction from unknown device ==> \n")); return; } if (pP2PCtrl->bP2pReSendTimerRunning) { pP2PCtrl->bP2pReSendTimerRunning = FALSE; pAd->P2pTable.Client[p2pindex].ReTransmitCnt = 0; RTMPCancelTimer(&pP2PCtrl->P2pReSendTimer, &Cancelled); } if (pAd->P2pCfg.GoFormCurrentState == P2P_WAIT_GO_FORM_PROV_RSP) pAd->P2pCfg.GoFormCurrentState = P2P_GO_FORM_IDLE; P2pStopConnectThis(pAd); pP2pEntry = &pAd->P2pTable.Client[p2pindex]; pP2pEntry->bValid = FALSE; DBGPRINT(RT_DEBUG_ERROR, ("P2pReceProvisionRspAction from P2P_Tab[%d] = %02x:%02x:%02x:%02x:%02x:%02x ==> \n", p2pindex, PRINT_MAC(pP2pEntry->addr))); /* point to OUI2 */ Ptr += (sizeof(HEADER_802_11) + 2 + 4 + 4); /* Category + Action + OUI + OUIType + SubType + Token + ElementID + Length */ if (RTMPEqualMemory(Ptr, WPS_OUI, 4)) { /* point to Element ID */ /*Ptr -= 2;*/ P2PParseWPSIE(Ptr, (pFrame->Length +2), NULL, &ConfigMethod, NULL, NULL); } DBGPRINT(RT_DEBUG_ERROR, ("P2pRece Provision RspAction ==>ConfigMethod = %04x. %s.\n", ConfigMethod, decodeConfigMethod(ConfigMethod))); DBGPRINT(RT_DEBUG_ERROR, ("pAd->P2pCfg.Dpid = %d. %s.\n", pAd->P2pCfg.Dpid, decodeDpid(pAd->P2pCfg.Dpid))); /* Set Event to GUI to display PIN digit. */ if (ConfigMethod == WSC_CONFMET_KEYPAD) { DBGPRINT(RT_DEBUG_ERROR, ("P2pKeSetEvent 13. Show PIN now\n")); } if (pP2pEntry->P2pClientState == P2PSTATE_SENT_PROVISION_REQ) { if (pP2pEntry->Rule == P2P_IS_GO) pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY_GO; else pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY; } /* p.s spec said null indicates failure. So check ConfigMethod to decide if should go on following actions. */ if ((ConfigMethod == WSC_CONFMET_DISPLAY) || (ConfigMethod == WSC_CONFMET_KEYPAD) || (ConfigMethod == WSC_CONFMET_PBC)) { BOOLEAN bAutoGroupFormation = FALSE; P2P_SET_FLAG(&pAd->P2pTable.Client[p2pindex], P2PFLAG_PROVISIONED); if (pP2pEntry->ConfigMethod == 0) bAutoGroupFormation = TRUE; pP2pEntry->ConfigMethod = ConfigMethod; P2P_SetWscRule(pAd, p2pindex, &PeerWscMethod); /* Peer use Label or Display. If peer is alreayd GO, I alreayd have WPS information to connect */ /* So doesn't need to pop up a setting windows. */ if ((ConfigMethod == WSC_CONFMET_DISPLAY) && (pP2pEntry->Rule != P2P_IS_GO)) { DBGPRINT(RT_DEBUG_TRACE, ("Need setting first. Pop up setting windows. %d\n", pAd->P2pCfg.PopUpIndex)); pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY; } DBGPRINT(RT_DEBUG_ERROR, ("Peer %d ConfigMethod = %s \n", p2pindex, decodeConfigMethod(ConfigMethod))); if (pP2pEntry->Rule == P2P_IS_GO) { DBGPRINT(RT_DEBUG_TRACE, ("Decide to Join P2p group? when I am %s \n", decodeP2PState(pAd->P2pCfg.P2PConnectState))); if (pAd->P2pCfg.P2PConnectState == P2P_CONNECT_IDLE) { DBGPRINT(RT_DEBUG_TRACE, ("P2p :connecting to GO with Bssid %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(pAd->P2pTable.Client[p2pindex].bssid))); DBGPRINT(RT_DEBUG_TRACE, ("P2p : its GroupCapability= %x. DevCapability= %x. \n", pAd->P2pTable.Client[p2pindex].GroupCapability, pAd->P2pTable.Client[p2pindex].DevCapability)); pP2PCtrl->P2PConnectState = P2P_ANY_IN_FORMATION_AS_CLIENT; pP2pEntry->P2pClientState = P2PSTATE_GO_WPS; RTMPMoveMemory(pP2PCtrl->Bssid, pAd->P2pTable.Client[p2pindex].bssid, MAC_ADDR_LEN); RTMPMoveMemory(pAd->P2pCfg.Bssid, pP2pEntry->bssid, MAC_ADDR_LEN); RTMPZeroMemory(pP2PCtrl->SSID, 32); RTMPMoveMemory(pP2PCtrl->SSID, pP2pEntry->Ssid, pP2pEntry->SsidLen); pP2PCtrl->SSIDLen = pP2pEntry->SsidLen; pP2PCtrl->GroupOpChannel = pP2pEntry->OpChannel; DBGPRINT(RT_DEBUG_TRACE, ("Peer %d pP2pEntry->OpChannel = %d \n", p2pindex, pP2pEntry->OpChannel)); P2pGoNegoDone(pAd, pP2pEntry); } } else { if (P2P_GO_ON(pAd)) { /* Set MyRule in P2P GroupFormat */ P2pSetWps(pAd, pP2pEntry); DBGPRINT(RT_DEBUG_ERROR, (" P2P - I am Internal REGISTRA!! REGISTRA. !! GOGOGO\n")); } else { if ((pP2PCtrl->ConfigMethod == WSC_CONFMET_KEYPAD) || ((pP2PCtrl->ConfigMethod == WSC_CONFMET_DISPLAY) && bAutoGroupFormation == FALSE)) { DBGPRINT(RT_DEBUG_ERROR, ("%s:: recv Provision Rsp. Condif Method = %04x.\n", __FUNCTION__, ConfigMethod)); } else { /* After Provision Success, start connect command */ pAddr = &pAd->P2pTable.Client[p2pindex].addr; P2pConnectPrepare(pAd, pAddr, P2PSTATE_CONNECT_COMMAND); } } } #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_RECV_PROV_RSP, &pAd->P2pTable.Client[p2pindex], pFrame->p80211Header.Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ } else { pP2pEntry->ConfigMethod = 0; #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT P2pSendWirelessEvent(pAd, RT_P2P_RECV_PROV_RSP, &pAd->P2pTable.Client[p2pindex], pFrame->p80211Header.Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ } } /* ========================================================================== Description: The routine send additional Probe Request when being in P2P Search State.. Parameters: Note: ========================================================================== */ VOID P2pSendProbeReq( IN PRTMP_ADAPTER pAd, IN UCHAR Channel) { PUCHAR pOutBuffer; ULONG FrameLen; NDIS_STATUS NStatus; MLME_QUEUE_ELEM *Elem; os_alloc_mem(pAd, (UCHAR **)&Elem, sizeof(MLME_QUEUE_ELEM)); if (Elem) { /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) { os_free_mem(NULL, Elem); return; } P2PMakeProbe(pAd, Elem, Channel, SUBTYPE_PROBE_REQ, pOutBuffer, &FrameLen); if (FrameLen > 0) MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen); MlmeFreeMemory(pAd, pOutBuffer); } os_free_mem(NULL, Elem); } /* ========================================================================== Description: The routine send Go Negociation Confirm packet . Parameters: Note: ========================================================================== */ VOID P2PSendGoNegoConfirm( IN PRTMP_ADAPTER pAd, IN UCHAR Token, IN UCHAR idx, IN PUCHAR Addr1) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; ULONG FrameLen; ULONG TempLen; PRT_P2P_CLIENT_ENTRY pP2pEntry; NDIS_STATUS NStatus; PUCHAR pOutBuffer = NULL, pDest; UCHAR Channel, StatusCode; PP2P_PUBLIC_FRAME pFrame; ULONG P2pLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; pP2pEntry = &pAd->P2pTable.Client[idx]; pDest = pOutBuffer; pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; P2PMakeGoNegoConfirm(pAd, Addr1, Token, pDest, &TempLen); FrameLen = TempLen; pDest += TempLen; P2pLen = 4; StatusCode = 0; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_STATUS, &StatusCode, pDest); pDest += TempLen; FrameLen += TempLen; P2pLen +=TempLen; /* : . */ TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CAP, pP2PCtrl->P2pCapability, pDest); FrameLen += TempLen; pDest += TempLen; P2pLen +=TempLen; /* If I am Go, Use my channel to set in the Go Nego Rsp. */ if (pP2pEntry->Rule == P2P_IS_CLIENT) { if (INFRA_ON(pAd)) Channel = pAd->CommonCfg.Channel; else Channel = pP2PCtrl->GroupChannel; DBGPRINT(RT_DEBUG_ERROR, ("%s:: P2P - I become Internal REGISTRA!! REGISTRA. !! GOGOGO\n", __FUNCTION__)); TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &Channel, pDest); FrameLen += TempLen; pDest += TempLen; P2pLen +=TempLen; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CHANNEL_LIST, &Channel, pDest); FrameLen += TempLen; pDest += TempLen; P2pLen +=TempLen; /* Shall include Group ID if I am GO */ /*NdisMoveMemory(pP2PCtrl->SSID, "DIRECT-qq", 9); */ /*pP2PCtrl->SSIDLen = 9; */ RTMPMoveMemory(pP2PCtrl->SSID, &pAd->ApCfg.MBSSID[MAIN_MBSSID].Ssid, pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen); pP2PCtrl->SSIDLen = pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen; { UCHAR tmpGroupID[40]; // 2 (Length) + 6 (MAC Address ) + 32 (SSID) tmpGroupID[0] = 6 + pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen; tmpGroupID[1] = 0; RTMPMoveMemory(&tmpGroupID[2], pP2PCtrl->CurrentAddress, 6); RTMPMoveMemory(&tmpGroupID[8], &pAd->ApCfg.MBSSID[MAIN_MBSSID].Ssid, pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen); TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_ID, &tmpGroupID, pDest); } FrameLen += TempLen; pDest += TempLen; P2pLen +=TempLen; pP2PCtrl->Rule = P2P_IS_GO; pAd->flg_p2p_OpStatusFlags = P2P_GO_UP; } else { /* update GO's SSID */ RTMPZeroMemory(pP2PCtrl->SSID, 32); RTMPMoveMemory(pP2PCtrl->SSID, pP2pEntry->Ssid, pP2pEntry->SsidLen); pP2PCtrl->SSIDLen = pP2pEntry->SsidLen; Channel = pP2pEntry->OpChannel; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &Channel, pDest); FrameLen += TempLen; pDest += TempLen; P2pLen +=TempLen; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CHANNEL_LIST, &Channel, pDest); FrameLen += TempLen; pDest += TempLen; P2pLen +=TempLen; DBGPRINT(RT_DEBUG_ERROR, ("%s:: P2P - I become ENROLLEE!! ENROLLEE.!!\n", __FUNCTION__)); /* *pDest = SUBID_P2P_CHANNEL_LIST; pDest++; *pDest = 6; *(pDest + 1) = 0; //country string . Two ASCII + one more byte *(pDest + 2) = 0x55; *(pDest + 3) = 0x53; *(pDest + 4) = 0x20; *(pDest + 5) = 3; if (Channel <= 11) { *(pDest + 5) = 12; } else if (Channel <= 48) { *(pDest + 5) = 1; } else if (Channel <= 64) { *(pDest + 5) = 2; } else if (Channel <= 140) { *(pDest + 5) = 4; } else if (Channel == 165) { *(pDest + 5) = 5; } *(pDest + 6) = 1; *(pDest + 7) = Channel; P2pLen += 9; FrameLen += 9; pDest += 9; // Insert Channel List Attribute. *pDest = SUBID_P2P_CHANNEL_LIST; pDest++; *pDest = 6; *(pDest + 1) = 0; */ pP2PCtrl->Rule = P2P_IS_CLIENT; pAd->flg_p2p_OpStatusFlags = P2P_CLI_UP; } DBGPRINT(RT_DEBUG_INFO, ("%s:: P2P - Opchannel is %d \n", __FUNCTION__, Channel)); pFrame->Length = (UCHAR)P2pLen; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &TempLen); pDest += TempLen; FrameLen += TempLen; } #endif /* WFD_SUPPORT */ pP2pEntry->P2pClientState = P2PSTATE_WAIT_GO_COMFIRM_ACK; /* PFP2P */ pP2pEntry->StateCount = 0; pP2pEntry->bValid = FALSE; /*P2pSetWps(pAd, pPort); */ MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen); MlmeFreeMemory(pAd, pOutBuffer); /* Note: Send event will take long time, so we send event only after GO-Neg-Confirm is sent out. */ #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT if (pP2pEntry->Rule == P2P_IS_CLIENT) { P2pSendWirelessEvent(pAd, RT_P2P_CONNECTED, pP2pEntry, pP2PCtrl->CurrentAddress); //to inform UI, Group Started and as GO. } #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ /* Wait here for waiting this GO Nego Confirm is sent out; otherwise this GO Nego Confirm will be sent out in operation channel. P2pSetWps will do AsicSwitchChannel to operation channel. */ OS_WAIT(50); /* Start WPS. */ P2pSetWps(pAd, pP2pEntry); } /* ========================================================================== Description: The routine make Device Discovery Request Action Frame Packet . Parameters: Note: ========================================================================== */ VOID P2PSendDevDisReq( IN PRTMP_ADAPTER pAd, IN PUCHAR Addr1, IN PUCHAR Bssid, IN PUCHAR ClientAddr1, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame; ULONG P2pIeLen = 0; PUCHAR pDest; ULONG Length; PUCHAR pOutBuffer = NULL; NDIS_STATUS NStatus; UCHAR i; PUCHAR Ptr = NULL; *pTotalFrameLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /*Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; pDest = pOutBuffer; pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Bssid); DBGPRINT(RT_DEBUG_TRACE, ("P2P - P2PSendDevDisReq TO %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(Addr1))); DBGPRINT(RT_DEBUG_TRACE, ("P2P - P2PSendDevDisReq Bssid %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(Bssid))); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = P2P_DEV_DIS_REQ; pP2PCtrl->Token++; pFrame->Token = pP2PCtrl->Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); P2pIeLen = 4; *pTotalFrameLen = 38; pDest = &pFrame->Octet[0]; /* attach subelementID = 3. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_ID, ClientAddr1, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; pFrame->Length = (UCHAR)P2pIeLen; pDest = (PUCHAR)pOutBuffer; for (i = 0; i <*pTotalFrameLen; ) { DBGPRINT(RT_DEBUG_TRACE,(": %x %x %x %x %x %x %x %x %x \n", *(pDest+i), *(pDest+i+1), *(pDest+i+2), *(pDest+i+3), *(pDest+i+4), *(pDest+i+5), *(pDest+i+6), *(pDest+i+7), *(pDest+i+8))); i = i + 9; } MiniportMMRequest(pAd, 0, pOutBuffer, *pTotalFrameLen); MlmeFreeMemory(pAd, pOutBuffer); DBGPRINT(RT_DEBUG_TRACE, ("P2P - P2PSendDevice Discovery Req . TO %02x:%02x:%02x:%02x:%02x:%02x. *pTotalFrameLen = %ld. \n", PRINT_MAC(Addr1), *pTotalFrameLen)); } /* ========================================================================== Description: The routine make Device Discovery Response Action Frame Packet . Send by GO. So Bssid is my GO addr Parameters: Note: ========================================================================== */ VOID P2PSendDevDisRsp( IN PRTMP_ADAPTER pAd, IN UCHAR RspStatus, IN UCHAR Token, IN PUCHAR Addr1, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame; ULONG P2pIeLen = 0; PUCHAR pDest; ULONG Length; PUCHAR pOutBuffer = NULL; NDIS_STATUS NStatus; PUCHAR Ptr = NULL; *pTotalFrameLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; pDest = pOutBuffer; pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, pP2PCtrl->CurrentAddress); DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2PSendDevDisRsp TO %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(Addr1))); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = P2P_DEV_DIS_RSP; pFrame->Token = Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); P2pIeLen = 4; *pTotalFrameLen = 38; pDest = &pFrame->Octet[0]; /* attach subelementID = 3. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_STATUS, &RspStatus, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; pFrame->Length = (UCHAR)P2pIeLen; MiniportMMRequest(pAd, 0, pOutBuffer, *pTotalFrameLen); MlmeFreeMemory(pAd, pOutBuffer); DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2PSendDevice Discovery Response. TO %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(Addr1))); } /* ========================================================================== Description: The routine make Invitiation Response Action Frame Packet . Parameters: Note: ========================================================================== */ VOID P2PSendProvisionReq( IN PRTMP_ADAPTER pAd, IN USHORT ConfigMethod, IN UCHAR Token, IN PUCHAR Addr1, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame; ULONG WpsIeLen = 0; PUCHAR pDest; PUCHAR pP2PIeLenDest; ULONG Length; PUCHAR pOutBuffer = NULL; NDIS_STATUS NStatus; PUCHAR Ptr = NULL; UCHAR index; PRT_P2P_CLIENT_ENTRY pP2pEntry; *pTotalFrameLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2PMakeProvisionReq . TO %02x:%02x:%02x:%02x:%02x:%02x. ConfigMethod = %s \n", PRINT_MAC(Addr1), decodeConfigMethod(ConfigMethod))); pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Addr1); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = P2P_PROVISION_REQ; pFrame->Token = Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), WPS_OUI, 4);*/ RTMPMoveMemory(Ptr, WPS_OUI, 4); WpsIeLen = 4; *pTotalFrameLen = 38; pDest = pFrame->Octet; /* attach config method . */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_CONFIG_METHODS); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0002); *((PUSHORT) (pDest + 4)) = cpu2be16(ConfigMethod); /* Label, Display, PBC */ pDest += 6; WpsIeLen += 6; *pTotalFrameLen += 6; pFrame->Length = (UCHAR)WpsIeLen; /* Insert P2P IE===> */ *pDest = IE_VENDOR_SPECIFIC; pP2PIeLenDest = (pDest+1); RTMPMoveMemory(pDest+2, P2POUIBYTE, 4); WpsIeLen = 4; pDest += 6; *pTotalFrameLen += 6; /* attach capability */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CAP, pP2PCtrl->P2pCapability, pDest); pDest += Length; WpsIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 13. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_INFO, pP2PCtrl->CurrentAddress, pDest); pDest += Length; WpsIeLen += Length; *pP2PIeLenDest = (UCHAR)WpsIeLen; *pTotalFrameLen += Length; /* attach subelementID = 15 */ index = P2pGroupTabSearch(pAd, Addr1); if (index != P2P_NOT_FOUND) { UCHAR tmpGroupID[40]; // 2 (Length) + 6 (MAC Address ) + 32 (SSID) pP2pEntry = &pAd->P2pTable.Client[index]; DBGPRINT(RT_DEBUG_TRACE, ("P2P - P2PMakeProvisionReq MyRule=%s, PeerRule=%d \n", decodeMyRule(pP2PCtrl->Rule), pP2pEntry->Rule)); if (pP2pEntry->Rule == P2P_IS_GO) // peer is GO { tmpGroupID[0] = 6 + pP2pEntry->SsidLen; tmpGroupID[1] = 0; RTMPMoveMemory(&tmpGroupID[2], pP2pEntry->addr, 6); RTMPMoveMemory(&tmpGroupID[8], &pP2pEntry->Ssid[0], pP2pEntry->SsidLen); Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_ID, &tmpGroupID, pDest); }else if (P2P_GO_ON(pAd)) //I'm GO { tmpGroupID[0] = 6 + pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen; tmpGroupID[1] = 0; RTMPMoveMemory(&tmpGroupID[2], pP2PCtrl->CurrentAddress, 6); RTMPMoveMemory(&tmpGroupID[8], &pAd->ApCfg.MBSSID[MAIN_MBSSID].Ssid, pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen); Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_ID, &tmpGroupID, pDest); } else { Length =0; } } else { Length =0; } pDest += Length; WpsIeLen += Length; *pP2PIeLenDest = (UCHAR)WpsIeLen; *pTotalFrameLen += Length; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &Length); pDest += Length; *pTotalFrameLen += Length; } #endif /* WFD_SUPPORT */ pP2PCtrl->bP2pReSendTimerRunning = TRUE; RTMPSetTimer(&pP2PCtrl->P2pReSendTimer, P2P_TRANSMIT_TIMER); MiniportMMRequest(pAd, 0, pOutBuffer, *pTotalFrameLen); MlmeFreeMemory(pAd, pOutBuffer); } /* ========================================================================== Description: The routine make Invitiation Response Action Frame Packet . Parameters: Note: ========================================================================== */ VOID P2PSendProvisionRsp( IN PRTMP_ADAPTER pAd, IN USHORT ConfigMethod, IN UCHAR Token, IN PUCHAR Addr1, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame; UCHAR WpsIeLen = 0; PUCHAR pDest; //ULONG Length; PUCHAR pOutBuffer = NULL; NDIS_STATUS NStatus; PUCHAR Ptr = NULL; *pTotalFrameLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2PMakeProvisionRsp . TO %02x:%02x:%02x:%02x:%02x:%02x. ConfigMethod = %x(%s) \n", PRINT_MAC(Addr1), ConfigMethod, decodeConfigMethod(ConfigMethod))); pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Addr1); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4); */ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = P2P_PROVISION_RSP; pFrame->Token = Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), WPS_OUI, 4); */ RTMPMoveMemory(Ptr, WPS_OUI, 4); WpsIeLen = 4; *pTotalFrameLen = 38; pDest = pFrame->Octet; if (ConfigMethod > 0) { /* attach wsc version */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_VERSION); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0001); *(pDest + 4) = WSC_VERSION; /* TODO: WPS2.0 ?? */ pDest += 5; WpsIeLen += 5; *pTotalFrameLen += 5; /* attach config method . */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_CONFIG_METHODS); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0002); *((PUSHORT) (pDest + 4)) = cpu2be16(ConfigMethod); /* Label, Display, PBC */ pDest += 6; WpsIeLen += 6; *pTotalFrameLen += 6; } #ifdef WFD_SUPPORT { ULONG WfdIeLen = 0, WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO) | (0x1 << SUBID_WFD_SESSION_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &WfdIeLen); pDest += WfdIeLen; *pTotalFrameLen += WfdIeLen; } #endif /* WFD_SUPPORT */ pFrame->Length = WpsIeLen; pP2PCtrl->bP2pReSendTimerRunning = TRUE; RTMPSetTimer(&pP2PCtrl->P2pReSendTimer, /*P2P_TRANSMIT_TIMER*/ 1000); MiniportMMRequest(pAd, 0, pOutBuffer, *pTotalFrameLen); MlmeFreeMemory(pAd, pOutBuffer); } /* ========================================================================== Description: The routine prepares Go Negociation Confirm packet . Parameters: Note: ========================================================================== */ VOID P2PMakeGoNegoConfirm( IN PRTMP_ADAPTER pAd, IN PUCHAR Addr1, IN UCHAR Token, IN PUCHAR pOutBuffer, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; /*PUCHAR pDest; */ /*ULONG Length; */ PUCHAR Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, (PHEADER_802_11)pOutBuffer, Addr1, pP2PCtrl->CurrentAddress, pP2PCtrl->PortCfg.Bssid); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4); */ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = GO_NEGOCIATION_CONFIRM; pFrame->Token = Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->Octet), P2POUIBYTE, 4); */ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); *pTotalFrameLen = 38; } /* ========================================================================== Description: The routine make Go Negociation Response packet . Parameters: TempIntent is directly from Go Req. Still need to parse this. Note: ========================================================================== */ VOID P2PMakeGoNegoRsp( IN PRTMP_ADAPTER pAd, IN PUCHAR Addr1, IN USHORT SentDpid, IN UCHAR Token, IN UCHAR TempIntent, IN UCHAR Channel, IN UCHAR Status, IN PUCHAR pOutBuffer, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; PUCHAR pDest; ULONG Length; /*UCHAR P2PCap[2]; */ ULONG P2pIeLen = 0; UCHAR RealIntent; UCHAR WscIEFixed[] = {0xdd, 0x0f, 0x00, 0x50, 0xf2, 0x04}; /* length will modify later */ PUCHAR Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Addr1); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* pointer to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr +=4; /* OUISubtype */ pFrame->Subtype = GO_NEGOCIATION_RSP; pFrame->Token = Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->Octet), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); P2pIeLen = 4; *pTotalFrameLen = sizeof(P2P_PUBLIC_FRAME) - 1; pDest = pFrame->Octet; /* Start attach subelement. */ /* attach subelementID= 0. */ DBGPRINT(RT_DEBUG_TRACE, ("%s: P2P - Status %d \n", __FUNCTION__, Status)); Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_STATUS, &Status, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 2. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CAP, pP2PCtrl->P2pCapability, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 4. */ RealIntent = TempIntent>>1; RealIntent = (pP2PCtrl->GoIntentIdx << 1) + ((TempIntent & 0x1) == 1 ? 0 : 1); Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OWNER_INTENT, &RealIntent, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 4. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CONFIG_TIMEOUT, &pP2PCtrl->ConfigTimeout[0], pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 17. */ if (INFRA_ON(pAd)) Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pAd->CommonCfg.Channel, pDest); else Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pP2PCtrl->GroupChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 9. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_INTERFACE_ADDR, pP2PCtrl->CurrentAddress, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach channel List= 11. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CHANNEL_LIST, &pP2PCtrl->GroupChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 13. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_INFO, pP2PCtrl->CurrentAddress, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 15. */ if (pP2PCtrl->P2PConnectState == P2P_ANY_IN_FORMATION_AS_GO) { UCHAR tmpGroupID[40]; // 2 (Length) + 6 (MAC Address ) + 32 (SSID) tmpGroupID[0] = 6 + pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen; tmpGroupID[1] = 0; RTMPMoveMemory(&tmpGroupID[2], pP2PCtrl->CurrentAddress, 6); RTMPMoveMemory(&tmpGroupID[8], &pAd->ApCfg.MBSSID[MAIN_MBSSID].Ssid, pAd->ApCfg.MBSSID[MAIN_MBSSID].SsidLen); Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_ID, &tmpGroupID, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; } pFrame->Length = (UCHAR)P2pIeLen; /* 0. WSC fixed IE */ RTMPMoveMemory(pDest, &WscIEFixed[0], 6); pDest += 6; *pTotalFrameLen += 6; /* 1. Version */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_VERSION); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0001); *(pDest + 4) = WSC_VERSION; pDest += 5; *pTotalFrameLen += 5; /* 9. Device password ID. According to Table.1 in P2P Spec. */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_DEVICE_PWD_ID); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0002); *((PUSHORT) (pDest + 4)) = cpu2be16(SentDpid); pDest += 6; *pTotalFrameLen += 6; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &Length); pDest += Length; *pTotalFrameLen += Length; } #endif /* WFD_SUPPORT */ } /* ========================================================================== Description: The routine make Go Negociation Request packet . Parameters: Note: ========================================================================== */ VOID P2PMakeGoNegoReq( IN PRTMP_ADAPTER pAd, IN UCHAR index, IN PUCHAR Addr1, IN PUCHAR pOutBuffer, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; PUCHAR pDest; ULONG Length; ULONG P2pIeLen = 0; UCHAR FilledIntent; UCHAR RandomB; UCHAR WscIEFixed[] = {0xdd, 0x0f, 0x00, 0x50, 0xf2, 0x04}; /* length will modify later */ PUCHAR pWscIeLen; /*UCHAR ChannelList; */ USHORT SentDpid; PUCHAR Ptr = (PUCHAR) pFrame; if (index >= MAX_P2P_GROUP_SIZE) { return; } *pTotalFrameLen = 0; RTMPMoveMemory(pP2PCtrl->PortCfg.Bssid, Addr1, 6); ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Addr1); DBGPRINT(RT_DEBUG_ERROR, (" %s - P2PMakeGoNegoReq TO %02x:%02x:%02x:%02x:%02x:%02x. \n", __FUNCTION__, Addr1[0], Addr1[1], Addr1[2],Addr1[3],Addr1[4],Addr1[5])); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = GO_NEGOCIATION_REQ; pFrame->Token = 1; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); P2pIeLen = 4; *pTotalFrameLen = 38; pDest = pFrame->Octet; /* Start attach subelement. */ /* attach subelementID= 2. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CAP, pP2PCtrl->P2pCapability, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 4. */ FilledIntent = pP2PCtrl->GoIntentIdx<<1; RandomB = RandomByte(pAd); /* If Intent is not 15, set tie breaker bit to 1 randomly. */ if (((RandomB %2) == 0) && (pP2PCtrl->GoIntentIdx != 15)) FilledIntent |= 1; Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OWNER_INTENT, &FilledIntent, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 7. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CONFIG_TIMEOUT, &pP2PCtrl->ConfigTimeout[0], pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_LISTEN_CHANNEL, &pP2PCtrl->ListenChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 9. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_INTERFACE_ADDR, pP2PCtrl->CurrentAddress, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach channel List= 11. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CHANNEL_LIST, &pP2PCtrl->GroupChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 13. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_INFO, pP2PCtrl->CurrentAddress, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 17. */ if (INFRA_ON(pAd)) Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pAd->CommonCfg.Channel, pDest); else Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pP2PCtrl->GroupChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; if (IS_P2P_SUPPORT_EXT_LISTEN(pAd)) { /* attach subelementID= 17. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_EXT_LISTEN_TIMING, NULL, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; } pFrame->Length = (UCHAR)P2pIeLen; /* 0. WSC fixed IE */ RTMPMoveMemory(pDest, &WscIEFixed[0], 6); pWscIeLen = pDest + 1; pDest += 6; *pTotalFrameLen += 6; *pWscIeLen = 4; /* 1. Version */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_VERSION); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0001); *(pDest + 4) = WSC_VERSION; pDest += 5; *pTotalFrameLen += 5; *pWscIeLen += 5; /* 9. Device password ID */ *((PUSHORT) pDest) = cpu2be16(WSC_ID_DEVICE_PWD_ID); *((PUSHORT) (pDest + 2)) = cpu2be16(0x0002); SentDpid = pP2PCtrl->Dpid; /* The Dpid have been updated when receiving provision request or response frame */ *((PUSHORT) (pDest + 4)) = cpu2be16(SentDpid); pDest += 6; *pTotalFrameLen += 6; *pWscIeLen += 6; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &Length); pDest += Length; *pTotalFrameLen += Length; } #endif /* WFD_SUPPORT */ } /* ========================================================================== Description: The routine make Invitiation Request Action Frame Packet . Parameters: Note: ========================================================================== */ VOID P2PMakeInviteReq( IN PRTMP_ADAPTER pAd, IN UCHAR MyRule, IN UCHAR InviteFlag, IN PUCHAR Addr1, IN PUCHAR Bssid, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame; ULONG P2pIeLen = 0; PUCHAR pDest; ULONG Length; PUCHAR pOutBuffer = NULL; NDIS_STATUS NStatus; PUCHAR Ptr = NULL; *pTotalFrameLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; Ptr = (PUCHAR) pFrame; ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Bssid); DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2PMakeInviteReq TO %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(Addr1))); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = P2P_INVITE_REQ; pP2PCtrl->Token++; pFrame->Token = pP2PCtrl->Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); P2pIeLen = 4; *pTotalFrameLen = 38; pDest = pFrame->Octet; /* attach subelementID= 5. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CONFIG_TIMEOUT, &pP2PCtrl->ConfigTimeout[0], pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 11. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_INVITE_FLAG, &InviteFlag, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 17. */ if (MyRule == P2P_IS_GO) { if (INFRA_ON(pAd)) Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pAd->CommonCfg.Channel, pDest); else Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pP2PCtrl->GroupOpChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; } /* attach subelementID= 7. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_BSSID, &pP2PCtrl->CurrentAddress[0], pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 11. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CHANNEL_LIST, &pP2PCtrl->GroupOpChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 11. */ { UCHAR tmpGroupID[40]; // 2 (Length) + 6 (MAC Address ) + 32 (SSID) tmpGroupID[0] = 6 + pP2PCtrl->SSIDLen; tmpGroupID[1] = 0; RTMPMoveMemory(&tmpGroupID[2], pP2PCtrl->CurrentAddress, 6); RTMPMoveMemory(&tmpGroupID[8], &pP2PCtrl->SSID[0], pP2PCtrl->SSIDLen); Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_ID, &tmpGroupID, pDest); } pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 11. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_INFO, pP2PCtrl->CurrentAddress, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; pFrame->Length = (UCHAR)P2pIeLen; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO) | (0x1 << SUBID_WFD_SESSION_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &Length); pDest += Length; *pTotalFrameLen += Length; } #endif /* WFD_SUPPORT */ MiniportMMRequest(pAd, 0, pOutBuffer, *pTotalFrameLen); MlmeFreeMemory(pAd, pOutBuffer); } /* ========================================================================== Description: The routine make Invitiation Response Action Frame Packet . Parameters: Note: ========================================================================== */ VOID P2PMakeInviteRsp( IN PRTMP_ADAPTER pAd, IN UCHAR MyRule, IN UCHAR Token, IN PUCHAR Addr1, IN PUCHAR Bssid, IN PUCHAR OpChannel, IN PUCHAR Status, OUT PULONG pTotalFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PP2P_PUBLIC_FRAME pFrame; ULONG P2pIeLen = 0; PUCHAR pDest; ULONG Length; PUCHAR pOutBuffer = NULL; NDIS_STATUS NStatus; PUCHAR Ptr = NULL; *pTotalFrameLen = 0; /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) return; pFrame = (PP2P_PUBLIC_FRAME)pOutBuffer; Ptr = (PUCHAR) pFrame; if (MyRule == P2P_IS_CLIENT) ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Addr1); else ActHeaderInit(pAd, &pFrame->p80211Header, Addr1, pP2PCtrl->CurrentAddress, Bssid); DBGPRINT(RT_DEBUG_ERROR, ("P2P - P2PMakeInviteRsp MyRule = %s. TO %02x:%02x:%02x:%02x:%02x:%02x. \n", decodeMyRule(MyRule), PRINT_MAC(Addr1))); DBGPRINT(RT_DEBUG_ERROR, ("P2P - Bssid %02x:%02x:%02x:%02x:%02x:%02x. \n", PRINT_MAC(Bssid))); pFrame->Category = CATEGORY_PUBLIC; pFrame->Action = ACTION_WIFI_DIRECT; /* point to OUI */ Ptr += (sizeof(HEADER_802_11) + 2); /*RTMPMoveMemory((pFrame->OUI), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); Ptr += 4; /* OUISubtype */ pFrame->Subtype = P2P_INVITE_RSP; pFrame->Token = Token; pFrame->ElementID = IE_VENDOR_SPECIFIC; /* point to OUI2 */ Ptr += 4; /*RTMPMoveMemory((pFrame->OUI2), P2POUIBYTE, 4);*/ RTMPMoveMemory(Ptr, P2POUIBYTE, 4); P2pIeLen = 4; *pTotalFrameLen = 38; pDest = pFrame->Octet; /* attach subelementID= 0. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_STATUS, Status, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 5. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CONFIG_TIMEOUT, &pP2PCtrl->ConfigTimeout[0], pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; /* attach subelementID= 17. */ if ((MyRule == P2P_IS_GO) && (*Status == P2PSTATUS_SUCCESS)) { DBGPRINT(RT_DEBUG_TRACE, (" P2P - OpChannel = %d \n", pP2PCtrl->GroupChannel)); if (INFRA_ON(pAd)) Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, &pAd->CommonCfg.Channel, pDest); else Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, OpChannel, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; } /* attach subelementID= 7. */ if ((MyRule == P2P_IS_GO) && (*Status == P2PSTATUS_SUCCESS)) { Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_GROUP_BSSID, Bssid, pDest); pDest += Length; P2pIeLen += Length; *pTotalFrameLen += Length; } /* attach subelementID= 7. */ Length = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CHANNEL_LIST, OpChannel, pDest); pDest += Length; P2pIeLen += Length; /* attach subelementID= 17. */ *pTotalFrameLen += Length; pFrame->Length = (UCHAR)P2pIeLen; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO) | (0x1 << SUBID_WFD_SESSION_INFO); WfdMakeWfdIE(pAd, WfdIeBitmap, pDest, &Length); pDest += Length; *pTotalFrameLen += Length; } #endif /* WFD_SUPPORT */ MiniportMMRequest(pAd, 0, pOutBuffer, *pTotalFrameLen); MlmeFreeMemory(pAd, pOutBuffer); } /* ========================================================================== Description: Sanity check of the Probe Request frame when operating as a P2P Device. Can be called from both as AP's state machine or as STA( that is doing P2P search)'s state machine Parameters: Note: ========================================================================== */ BOOLEAN PeerP2pProbeReqSanity( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT PUCHAR pAddr2, OUT CHAR Ssid[], OUT UCHAR *pSsidLen, OUT ULONG *Peerip, OUT ULONG *P2PSubelementLen, OUT PUCHAR pP2pSubelement, #ifdef WFD_SUPPORT OUT ULONG *pWfdSubelementLen, OUT PUCHAR pWfdSubelement, #endif /* WFD_SUPPORT */ OUT ULONG *WpsIELen, OUT PUCHAR pWpsIE) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PFRAME_802_11 pFrame; PEID_STRUCT pEid; ULONG Length = 0; BOOLEAN brc = FALSE; UCHAR RateLen; #ifdef WFD_SUPPORT *pWfdSubelementLen = 0; #endif /* WFD_SUPPORT */ pFrame = (PFRAME_802_11)Msg; Length += LENGTH_802_11; *P2PSubelementLen = 0; *WpsIELen = 0; *pSsidLen = 0; *Peerip = 0; RateLen = 0; COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2); pEid = (PEID_STRUCT) pFrame->Octet; /* get variable fields from payload and advance the pointer */ while ((Length + 2 + pEid->Len) <= MsgLen) { switch(pEid->Eid) { case IE_EXT_SUPP_RATES: /* concatenate all extended rates to Rates[] and RateLen */ RateLen = (RateLen) + pEid->Len; break; case IE_SUPP_RATES: RateLen = (RateLen) + pEid->Len; break; case IE_SSID: if(pEid->Len <= MAX_LEN_OF_SSID) { RTMPMoveMemory(Ssid, pEid->Octet, pEid->Len); *pSsidLen = pEid->Len; } break; case IE_VENDOR_SPECIFIC: /* Check the OUI version, filter out non-standard usage */ if (NdisEqualMemory(pEid->Octet, WPS_OUI, 4) && (pEid->Len > 4)) { RTMPMoveMemory(pWpsIE, &pEid->Eid, pEid->Len +2); *WpsIELen = pEid->Len + 2; } if (NdisEqualMemory(pEid->Octet, P2POUIBYTE, 4) && (pEid->Len > 4)) { if (*P2PSubelementLen == 0) { RTMPMoveMemory(pP2pSubelement, &pEid->Eid, pEid->Len+2); *P2PSubelementLen = pEid->Len+2; brc = TRUE; } else if (*P2PSubelementLen > 0) { if (((*P2PSubelementLen) + (pEid->Len+2)) <= MAX_VIE_LEN) { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Eid, pEid->Len+2); *P2PSubelementLen += (pEid->Len+2); brc = TRUE; } else { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 111 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__ ,((*P2PSubelementLen) + (pEid->Len+2)))); return FALSE; } } DBGPRINT(RT_DEBUG_INFO, (" !FIND!!!!!!===>P2P - PeerP2pProbeReq P2P IE Len = %ld. %s\n", *P2PSubelementLen, decodeP2PState(pP2PCtrl->P2PConnectState))); } #ifdef WFD_SUPPORT else if ((pAd->StaCfg.WfdCfg.bWfdEnable) && NdisEqualMemory(pEid->Octet, WIFIDISPLAY_OUI, 4) && (pEid->Len >= 4)) { //hex_dump("WFD_IE", &pEid->Eid, pEid->Len +2); RTMPMoveMemory(pWfdSubelement, &pEid->Eid, pEid->Len +2); *pWfdSubelementLen = pEid->Len + 2; DBGPRINT(RT_DEBUG_TRACE, (" !FIND!!!!!!===>WFD - PeerP2pProbeReq WFD IE Len = %ld.\n", *pWfdSubelementLen)); } #endif /* WFD_SUPPORT */ #ifdef WFD_SUPPORT #endif /* WFD_SUPPORT */ break; } Length = Length + 2 + pEid->Len; /* Eid[1] + Len[1]+ content[Len] */ pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } /* Doesn't parse probe request that only support 11b. So return FALSE. */ if (RateLen <= 4) { brc = FALSE; DBGPRINT(RT_DEBUG_INFO, ("Ignore Probe Request that is 11b-only.\n")); } /* Check P2P's Probe Response validatity. */ if (*P2PSubelementLen <= 4) { brc = FALSE; DBGPRINT(RT_DEBUG_INFO, ("Ignore Probe Request that doesn't have P2P IE.\n")); } return brc; } /* ========================================================================== Description: Sanity check of the Probe Response frame when operating as a P2P Device. Only called from STA's state machine that is in scanning. Parameters: Note: ========================================================================== */ BOOLEAN PeerP2pBeaconSanity( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT PUCHAR pAddr2, OUT CHAR Ssid[], OUT UCHAR *pSsidLen, OUT ULONG *Peerip, OUT ULONG *P2PSubelementLen, OUT PUCHAR pP2pSubelement) { PFRAME_802_11 pFrame; PEID_STRUCT pEid; ULONG Length = 0; BOOLEAN brc = FALSE; PUCHAR Ptr; BOOLEAN bFirstP2pOUI = TRUE; pFrame = (PFRAME_802_11)Msg; Length += LENGTH_802_11; *P2PSubelementLen = 0; *pSsidLen = 0; *Peerip = 0; COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2); Ptr = pFrame->Octet; /* get timestamp from payload and advance the pointer */ Ptr += TIMESTAMP_LEN; Length += TIMESTAMP_LEN; /* get beacon interval from payload and advance the pointer */ Ptr += 2; Length += 2; /* get capability info from payload and advance the pointer */ Ptr += 2; Length += 2; pEid = (PEID_STRUCT) Ptr; /* get variable fields from payload and advance the pointer */ while ((Length + 2 + pEid->Len) <= MsgLen) { switch(pEid->Eid) { case IE_SSID: if(pEid->Len <= MAX_LEN_OF_SSID) { RTMPMoveMemory(Ssid, pEid->Octet, pEid->Len); *pSsidLen = pEid->Len; } break; case IE_VENDOR_SPECIFIC: /* Check the OUI version, filter out non-standard usage */ if (NdisEqualMemory(pEid->Octet, WPS_OUI, 4) && (pEid->Len >= 4)) { if (*P2PSubelementLen == 0) { RTMPMoveMemory(pP2pSubelement, &pEid->Eid, pEid->Len +2); *P2PSubelementLen = pEid->Len +2; } else if (*P2PSubelementLen > 0) { if (((*P2PSubelementLen) + (pEid->Len+2)) <= MAX_VIE_LEN) { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Eid, pEid->Len+2); *P2PSubelementLen += (pEid->Len+2); } else { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 111 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__, ((*P2PSubelementLen) + (pEid->Len+2)))); return FALSE; } } } else if (NdisEqualMemory(pEid->Octet, P2POUIBYTE, 4) && (pEid->Len >= 4)) { /* If this is the first P2P OUI. Then also append P2P OUI. Beacon 's P2P attribute doesn't exceed 256 bytes. So not use acumulcated form. */ if (bFirstP2pOUI == TRUE) { if (*P2PSubelementLen == 0) { RTMPMoveMemory(pP2pSubelement, &pEid->Eid, pEid->Len +2); *P2PSubelementLen = (pEid->Len +2); brc = TRUE; } else if (*P2PSubelementLen > 0) { if (((*P2PSubelementLen) + (pEid->Len+2)) <= MAX_VIE_LEN) { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Eid, pEid->Len+2); *P2PSubelementLen += (pEid->Len+2); brc = TRUE; } else { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 222 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__, ((*P2PSubelementLen) + (pEid->Len+2)))); return FALSE; } } bFirstP2pOUI = FALSE; } else { /* If this is not the first P2P OUI. Then don't append P2P OUI. because our parse function doesn't need so many P2P OUI. */ if ((*P2PSubelementLen > 0) && (pEid->Len > 4)) { if (((*P2PSubelementLen) + (pEid->Len+2)) <= MAX_VIE_LEN) { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Eid, pEid->Len+2); *P2PSubelementLen += (pEid->Len+2); brc = TRUE; } else { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 333 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__, ((*P2PSubelementLen) + (pEid->Len+2)))); return FALSE; } } } } break; } Length = Length + 2 + pEid->Len; /* Eid[1] + Len[1]+ content[Len] */ pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } return brc; } /* ========================================================================== Description: Sanity check of the Probe Response frame when operating as a P2P Device. Only called from STA's state machine that is in scanning. Parameters: Note: ========================================================================== */ #ifndef WFD_SUPPORT BOOLEAN PeerP2pProbeRspSanity( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT PUCHAR pAddr2, OUT CHAR Ssid[], OUT UCHAR *pSsidLen, OUT ULONG *Peerip, OUT UCHAR *pChannel, OUT ULONG *P2PSubelementLen, OUT PUCHAR pP2pSubelement) #else BOOLEAN PeerP2pProbeRspSanity( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT PUCHAR pAddr2, OUT CHAR Ssid[], OUT UCHAR *pSsidLen, OUT ULONG *Peerip, OUT UCHAR *pChannel, OUT ULONG *P2PSubelementLen, OUT PUCHAR pP2pSubelement, OUT ULONG *pWfdSubelementLen, OUT PUCHAR pWfdSubelement) #endif /* WFD_SUPPORT */ { PFRAME_802_11 pFrame; PEID_STRUCT pEid; ULONG Length = 0; BOOLEAN brc = FALSE; PUCHAR Ptr; BOOLEAN bFirstP2pOUI = TRUE; BOOLEAN bLastIsP2pOUI = FALSE; PUCHAR pP2PIeConLen = NULL; /* pointer to 2 bytes to indicate Contenated length of all P2P IE */ ULONG P2PIeConLen = 0; /* Contenated length of all P2P IE */ ULONG idx; pFrame = (PFRAME_802_11)Msg; Length = LENGTH_802_11; *P2PSubelementLen = 0; #ifdef WFD_SUPPORT *pWfdSubelementLen = 0; #endif /* WFD_SUPPORT */ *pSsidLen = 0; *Peerip = 0; COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2); Ptr = pFrame->Octet; /* get timestamp from payload and advance the pointer */ Ptr += TIMESTAMP_LEN; Length += TIMESTAMP_LEN; /* get beacon interval from payload and advance the pointer */ Ptr += 2; Length += 2; /* get capability info from payload and advance the pointer */ Ptr += 2; Length += 2; pEid = (PEID_STRUCT) Ptr; /* get variable fields from payload and advance the pointer */ while ((Length + 2 + pEid->Len) <= MsgLen) { switch(pEid->Eid) { case IE_SSID: bLastIsP2pOUI = FALSE; if(pEid->Len <= MAX_LEN_OF_SSID) { RTMPMoveMemory(Ssid, pEid->Octet, pEid->Len); *pSsidLen = pEid->Len; } break; case IE_DS_PARM: if(pEid->Len == 1) *pChannel = *pEid->Octet; break; case IE_VENDOR_SPECIFIC: bLastIsP2pOUI = FALSE; /* Check the OUI version, filter out non-standard usage */ if (NdisEqualMemory(pEid->Octet, WPS_OUI, 4) && (pEid->Len >= 4)) { if (*P2PSubelementLen == 0) { RTMPMoveMemory(pP2pSubelement, &pEid->Eid, pEid->Len +2); *P2PSubelementLen = pEid->Len +2; } else if (*P2PSubelementLen > 0) { if (((*P2PSubelementLen) + (pEid->Len+2)) <= MAX_VIE_LEN) //added boundary checking { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Eid, pEid->Len+2); *P2PSubelementLen += (pEid->Len+2); } else // reach the boundary { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 111 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__, ((*P2PSubelementLen) + (pEid->Len+2)))); return FALSE; } } } else if (NdisEqualMemory(pEid->Octet, P2POUIBYTE, 4) && (pEid->Len >= 4)) { brc = TRUE; bLastIsP2pOUI = TRUE; /* If this is the first P2P OUI. Then also append P2P OUI. */ if (bFirstP2pOUI == TRUE) { /* Althought this is first P2P IE. */ /* still need to Check *P2PSubelementLen, because *P2PSubelementLen also includes WPS IE. */ if (*P2PSubelementLen == 0) { RTMPMoveMemory(pP2pSubelement, &pEid->Eid, 2); *(pP2pSubelement + 2) = 0; /* Make one more byte for P2P accumulated length. */ RTMPMoveMemory(pP2pSubelement + 3, &pEid->Octet[0], pEid->Len); pP2PIeConLen = pP2pSubelement + *P2PSubelementLen + 1; *P2PSubelementLen = (pEid->Len + 3); P2PIeConLen = pEid->Len; /* Real P2P IE length is Len. */ DBGPRINT(RT_DEBUG_INFO, ("SYNC -1-1 P2PIeConLen = %ld\n", P2PIeConLen)); } else if (*P2PSubelementLen > 0) { if (((*P2PSubelementLen) + (pEid->Len+3)) <= MAX_VIE_LEN) { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Eid, 2); *(pP2pSubelement + *P2PSubelementLen + 2) = 0; /* Make one more byte for P2P accumulated length. */ RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen + 3, &pEid->Octet[0], pEid->Len); pP2PIeConLen = pP2pSubelement + *P2PSubelementLen + 1; *P2PSubelementLen += (pEid->Len+3); /* bFirstP2pOUI is TURE. So use = */ P2PIeConLen = pEid->Len; DBGPRINT(RT_DEBUG_INFO, (" -1-2 P2PIeConLen = %ld\n", P2PIeConLen)); } else { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 222 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__, ((*P2PSubelementLen) + (pEid->Len+3)))); return FALSE; } } bFirstP2pOUI = FALSE; } else if (bLastIsP2pOUI == TRUE) { /* If this is not the first P2P OUI. Then don't append P2P OUI. */ /* because our parse function doesn't need so many P2P OUI. */ if ((*P2PSubelementLen > 0) && (pEid->Len > 4)) { if (((*P2PSubelementLen) + (pEid->Len-4)) <= MAX_VIE_LEN) { RTMPMoveMemory(pP2pSubelement + *P2PSubelementLen, &pEid->Octet[4], pEid->Len-4); *P2PSubelementLen += (pEid->Len-4); P2PIeConLen += (pEid->Len - 4); } else { DBGPRINT(RT_DEBUG_ERROR, ("%s: ERROR!! 333 Sum of P2PSubelementLen= %lu, > MAX_VIE_LEN !!\n", __FUNCTION__, ((*P2PSubelementLen) + (pEid->Len-4)))); return FALSE; } } } } #ifdef WFD_SUPPORT else if ((pAd->StaCfg.WfdCfg.bWfdEnable) && NdisEqualMemory(pEid->Octet, WIFIDISPLAY_OUI, 4) && (pEid->Len >= 4)) { //hex_dump("WFD_IE", &pEid->Eid, pEid->Len +2); RTMPMoveMemory(pWfdSubelement, &pEid->Eid, pEid->Len +2); *pWfdSubelementLen = pEid->Len + 2; } #endif /* WFD_SUPPORT */ break; default : bLastIsP2pOUI = FALSE; break; } Length = Length + 2 + pEid->Len; /* Eid[1] + Len[1]+ content[Len] */ pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } if ((P2PIeConLen != 0) && (pP2PIeConLen != NULL)) { *pP2PIeConLen = (UCHAR)(P2PIeConLen%256); *(pP2PIeConLen+1) = (UCHAR)(P2PIeConLen/256); DBGPRINT(RT_DEBUG_INFO, (" - 3 P2PIeConLen = %ld. /256 = %ld. *P2PSubelementLen = %ld \n", P2PIeConLen, (P2PIeConLen/256), *P2PSubelementLen)); DBGPRINT(RT_DEBUG_INFO, (" - %x %x \n", *pP2PIeConLen, *(pP2PIeConLen+1) )); for (idx = 0; idx < (*P2PSubelementLen);) { DBGPRINT_RAW(RT_DEBUG_INFO, ("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-", *(pP2pSubelement+idx), *(pP2pSubelement+idx+1), *(pP2pSubelement+idx+2), *(pP2pSubelement+idx+3) ,*(pP2pSubelement+idx+4) ,*(pP2pSubelement+idx+5) ,*(pP2pSubelement+idx+6),*(pP2pSubelement+idx+7) ,*(pP2pSubelement+idx+8),*(pP2pSubelement+idx+9),*(pP2pSubelement+idx+10),*(pP2pSubelement+idx+11))); idx = idx + 12; } } return brc; } VOID P2pPeerBeaconAtJoinAction( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem, IN PUCHAR Bssid) { /*PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg;*/ UCHAR P2pManageability = 0xff; UCHAR NumOfOtherP2pAttri = 1; UCHAR Channel = 1; UCHAR TotalNumOfP2pAttribute = 1; if (Elem->MsgLen <= (LENGTH_802_11 + 12)) return; P2pParseManageSubElmt(pAd, &Elem->Msg[LENGTH_802_11 + 12], (Elem->MsgLen - LENGTH_802_11 - 12), &Channel, &NumOfOtherP2pAttri, &TotalNumOfP2pAttribute, &P2pManageability, NULL); DBGPRINT(RT_DEBUG_INFO, ("P2pParseManageSubElmt TotalNumOfP2pAttribute = %d. \n", TotalNumOfP2pAttribute)); /* If this AP carries Managed Attribute. Update to our ManageAPBSsid */ if (P2pManageability != 0xff) { DBGPRINT(RT_DEBUG_INFO, ("SYNC - Receive desired BEACON with valid P2pManageability= %x\n", P2pManageability)); DBGPRINT(RT_DEBUG_INFO, ("SYNC -NumOfOtherP2pAttri %d . Channel = %d. \n", NumOfOtherP2pAttri, Channel)); RTMPMoveMemory(pAd->P2pCfg.P2pManagedParm.ManageAPBSsid, Bssid, MAC_ADDR_LEN); /* Reset Minor Reason when connecting to Managed AP. */ pAd->P2pCfg.P2pManagedParm.APP2pMinorReason = 0; pAd->P2pCfg.P2pManagedParm.APUsageChannel = Channel ; /* If this is the latest managed AP that I connected to. but this AP turn off the managed function. */ /* I should clear my record. */ /* If this is a manged AP. update to the ManageAPBSsid. */ if ((P2P_TEST_BIT(P2pManageability, P2PMANAGED_ENABLE_BIT)) && (NumOfOtherP2pAttri == 0)) { DBGPRINT(RT_DEBUG_INFO, ("SYNC -I am connecting to a Managed AP. Save this Bssid. %x %x %x \n", Bssid[3],Bssid[4],Bssid[5])); } pAd->P2pCfg.P2pManagedParm.APP2pManageability = P2pManageability; } pAd->P2pCfg.P2pManagedParm.TotalNumOfP2pAttribute = TotalNumOfP2pAttribute; DBGPRINT(RT_DEBUG_INFO, ("SYNC - BEACON P2pManageability= %x. TotalNumOfP2pAttribute = %d. OtherP2pAttri = %d\n", P2pManageability, TotalNumOfP2pAttribute, NumOfOtherP2pAttri)); } /* ========================================================================== Description: Processing the Probe Response frame when operating as a P2P Device. Only called from STA's state machine that is in scanning. Parameters: Note: ========================================================================== */ VOID PeerP2pBeaconProbeRspAtScan( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) /* IN USHORT CapabilityInfo, */ /* IN UCHAR WorkingChannel) */ { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PRT_P2P_CLIENT_ENTRY pP2pEntry = NULL; BOOLEAN bBeacon = FALSE; BOOLEAN bresult; BOOLEAN bChangeState = FALSE; ULONG P2PSubelementLen; PUCHAR P2pSubelement = NULL; UCHAR index;/*, perstindex; */ UCHAR Addr2[6], SsidLen; UCHAR Ssid[32]; ULONG Peerip; UCHAR DevType[8], DevAddr[6], Channel = 0, OpChannel = 0, Intent; UCHAR GroupCap, DeviceCap, StatusCode; USHORT Dpid; UCHAR DsChannel = 0; PHEADER_802_11 pHeader; USHORT ConfigMethod = 0xffff; UCHAR DeviceNameLen = 0, DeviceName[32]; UCHAR SavedP2PTableNum; P2P_CMD_STRUCT P2pCmd; BOOLEAN bSendP2pEvent = FALSE; #ifdef WFD_SUPPORT ULONG WfdSubelementLen = 0; PUCHAR WfdSubelement = NULL; #endif /* WFD_SUPPORT */ /* If there is already BSS or IBSS and only one port, no need to parse Probe response for P2P discovery feature */ /* INFRA on includes case that I am P2P client. in this case, doesn't need to parse P2P IE in Probe Response either?? */ /* if ((INFRA_ON(pAd) || ADHOC_ON(pAd)) && (pAd->OpMode == OPMODE_STA)) { DBGPRINT(RT_DEBUG_ERROR, ("1 PeerP2pBeaconProbeRspAtScan:: %s \n", decodeP2PState(pP2PCtrl->P2PDiscoProvState))); return; } */ /* Init P2pSubelement */ os_alloc_mem(pAd, &P2pSubelement, MAX_VIE_LEN); if (P2pSubelement == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s::Allocate memory size(=%d) failed\n", __FUNCTION__, MAX_VIE_LEN)); goto CleanUp; } RTMPZeroMemory(&DeviceName[0], 32); #ifdef WFD_SUPPORT os_alloc_mem(pAd, &WfdSubelement, MAX_VIE_LEN); if (WfdSubelement == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s::Allocate memory size(=%d) failed\n", __FUNCTION__, MAX_VIE_LEN)); goto CleanUp; } #endif /* WFD_SUPPORT */ /* P2P Device's probe response doesn't set ess bit on. */ pHeader = (PHEADER_802_11)&Elem->Msg[0]; /* In P2P spec, the P2P IE in probe response and beacon are different. */ /* So set a boolean to check if this is a beacon frame. */ if (pHeader->FC.SubType == SUBTYPE_BEACON) bBeacon = TRUE; /* Intel put P2P IE into two separate IE */ /* Sanity check */ #ifndef WFD_SUPPORT if (PeerP2pProbeRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen, &Peerip, &DsChannel, &P2PSubelementLen, P2pSubelement)) #else if (PeerP2pProbeRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen, &Peerip, &DsChannel, &P2PSubelementLen, P2pSubelement, &WfdSubelementLen, WfdSubelement)) #endif /* WFD_SUPPORT */ { CHAR Rssi; /* Check P2P's Probe Response validatity. */ if (!(NdisEqualMemory(Ssid, &WILDP2PSSID[0], WILDP2PSSIDLEN))) { DBGPRINT(RT_DEBUG_INFO, ("PeerP2pBeaconProbeRspAtScan: Len = %ld.: \n", P2PSubelementLen)); DBGPRINT(RT_DEBUG_INFO, ("= %02x:%02x:%02x:%02x:%02x:%02x \n", PRINT_MAC(Addr2))); DBGPRINT(RT_DEBUG_INFO, ("= %c %c %c %c %c %c%c \n", Ssid[0], Ssid[1],Ssid[2],Ssid[3],Ssid[4],Ssid[5],Ssid[6])); goto CleanUp; } else DBGPRINT(RT_DEBUG_INFO, ("!! %s = %02x:%02x:%02x:%02x:%02x:%02x!!\n", __FUNCTION__, PRINT_MAC(Addr2))); Rssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0, Elem->AntSel, BW_20), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1, Elem->AntSel, BW_20), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2, Elem->AntSel, BW_20)); /* Step 1: Parse P2P attribute */ /* If this peer is provisioned, don't update Config */ if (bBeacon == FALSE) { P2pParseSubElmt(pAd, (PVOID)P2pSubelement, P2PSubelementLen, bBeacon, &Dpid, &GroupCap, &DeviceCap, DeviceName, &DeviceNameLen, DevAddr, NULL, NULL, NULL, NULL, &ConfigMethod, NULL, DevType, &Channel, &OpChannel, NULL, &Intent, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL,/*&WfdSubelementLen, WfdSubelement,*/ #endif /* WFD_SUPPORT */ NULL); } else { P2pParseSubElmt(pAd, (PVOID)P2pSubelement, P2PSubelementLen, bBeacon, &Dpid, &GroupCap, &DeviceCap, DeviceName, &DeviceNameLen, DevAddr, NULL, NULL, NULL, NULL, NULL, NULL, DevType, &Channel, &OpChannel, NULL, &Intent, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL,/*&WfdSubelementLen, WfdSubelement,*/ #endif /* WFD_SUPPORT */ NULL); } /* Step 3: Insert. */ SavedP2PTableNum = pAd->P2pTable.ClientNumber; index = P2pGroupTabSearch(pAd, DevAddr); if (index == P2P_NOT_FOUND) { /* Because only Probe Response carrys group info, */ /* If we use Probe Response to add to my P2P table structure. If this is beacon */ /* and in social channels. return. */ if (bBeacon == TRUE) goto CleanUp; DBGPRINT(RT_DEBUG_ERROR, ("2 New From Addr2 = %02x:%02x:%02x:%02x:%02x:%02x. bBeacon = %d . GroupCap = %x \n", PRINT_MAC(Addr2), bBeacon, GroupCap)); DBGPRINT(RT_DEBUG_ERROR, ("2 New From DevAddr = %02x:%02x:%02x:%02x:%02x:%02x. bBeacon = %d . GroupCap = %x \n", PRINT_MAC(DevAddr), bBeacon, GroupCap)); if ((bBeacon == TRUE) || ((GroupCap&GRPCAP_OWNER) == GRPCAP_OWNER)) { index = P2pGroupTabInsert(pAd, DevAddr, P2PSTATE_DISCOVERY_GO, Ssid, SsidLen, DeviceCap, GroupCap); } /* P2P client can't send probe response. So the probe response must be from P2P device. */ else { index = P2pGroupTabInsert(pAd, DevAddr, P2PSTATE_DISCOVERY, Ssid, SsidLen, DeviceCap, GroupCap); } bSendP2pEvent = TRUE; } /* No matter the index is from existing table, or newly added, always update Ssid Information If this peer is not connected. */ /* Step 4: Update table. */ if (index < MAX_P2P_GROUP_SIZE) { pP2pEntry = &pAd->P2pTable.Client[index]; DBGPRINT(RT_DEBUG_INFO, ("%s:: got ProbeResp.index = %d. statue = %s\n", __FUNCTION__, index, decodeP2PClientState(pAd->P2pTable.Client[index].P2pClientState))); pP2pEntry->DevCapability = DeviceCap; pP2pEntry->GroupCapability = GroupCap; pP2pEntry->Rssi = Rssi; /* Update Rule according to latest Probe Response or Beacon that I received. */ if ((bBeacon == TRUE) && ((pP2pEntry->P2pClientState == P2PSTATE_DISCOVERY_CLIENT) || (pP2pEntry->P2pClientState == P2PSTATE_DISCOVERY))) { /* If total P2P device number > 10, don't want keep update topology if topology change. */ if ((pAd->P2pTable.ClientNumber < 10) || (((pAd->P2pTable.ClientNumber%4) == 3)&&(pAd->P2pTable.ClientNumber >= 10))) bChangeState = TRUE; pP2pEntry->Rule = P2P_IS_GO; pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY_GO; DBGPRINT(RT_DEBUG_ERROR, ("3 From DevAddr = %02x:%02x:%02x:%02x:%02x:%02x. Change rule to GO GroupCap = %x \n", PRINT_MAC(DevAddr), GroupCap)); } else if ((pP2pEntry->P2pClientState == P2PSTATE_DISCOVERY_GO) && ((GroupCap&GRPCAP_OWNER) != GRPCAP_OWNER)) { if ((pAd->P2pTable.ClientNumber < 10) || (((pAd->P2pTable.ClientNumber % 4) == 3) && (pAd->P2pTable.ClientNumber >= 10))) bChangeState = TRUE; pP2pEntry->Rule = P2P_IS_CLIENT; pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY; DBGPRINT(RT_DEBUG_ERROR, ("4 From DevAddr = %02x:%02x:%02x:%02x:%02x:%02x. Change rule to Device. GroupCap = %x \n", PRINT_MAC(DevAddr), GroupCap)); } /* If peer is provisioned, don't update. We treat him only support this provisioned configmethod. */ if (!P2P_TEST_FLAG(pP2pEntry, P2PFLAG_PROVISIONED) && (ConfigMethod != 0xffff)) pP2pEntry->ConfigMethod = ConfigMethod; /* Always Update Device Name */ if (DeviceNameLen != 0) { RTMPMoveMemory(&pP2pEntry->DeviceName[0], DeviceName, 32); pP2pEntry->DeviceNameLen = DeviceNameLen; pP2pEntry->DeviceName[pP2pEntry->DeviceNameLen] = 0x0; } /* If this is beacon. It must use correct opchannel, ssid, interface addr, and SSID. So update. */ /* Step 4 - 1: Update table according to this is beacon or probe response. Beacon and probe response carries */ /* different attribute. */ if (bBeacon == TRUE) { pP2pEntry->Rule = P2P_IS_GO; RTMPMoveMemory(pP2pEntry->Ssid, Ssid, 32); pP2pEntry->SsidLen = SsidLen; RTMPMoveMemory(pP2pEntry->bssid, Addr2, MAC_ADDR_LEN); RTMPMoveMemory(pP2pEntry->InterfaceAddr, Addr2, MAC_ADDR_LEN); pP2pEntry->OpChannel = Elem->Priv; /* WorkingChannel; */ /* The peer is GO. So set its state to GO_WPS directly. No need for Group forming procedure. */ /* Search this beacon's group info, and insert its client to my p2p table too. */ if ((GroupCap & GRPCAP_OWNER) != GRPCAP_OWNER) { /* Print for debug. */ DBGPRINT(RT_DEBUG_ERROR, ("P2p : One P2P device[%d] send out Beacon. But group owner %x bit not set ? \n", index, GroupCap)); } } /* Make it more readable to use "else if". Group info only appears in Probe Response. */ else if (bBeacon == FALSE) { /* If this is GO. */ if ((GroupCap & GRPCAP_OWNER) == GRPCAP_OWNER) { /* The peer is GO. So set its state to GO_WPS directly. No need for Group forming procedure. */ pP2pEntry->Rule = P2P_IS_GO; } else { /* Not GO anymore, update State. */ if (IS_P2P_PEER_DISCOVERY(pP2pEntry)) pP2pEntry->P2pClientState = P2PSTATE_DISCOVERY; pP2pEntry->Rule = P2P_IS_CLIENT; } /* P2P PrimaryDevType only appears in Probe Response. not in beacon. */ RTMPMoveMemory(pP2pEntry->PrimaryDevType, DevType, 8); /* Can I just set this channel that I got Probe response as listent channel?? */ if (Elem->Priv) pP2pEntry->ListenChannel = Elem->Priv; bresult = P2pParseGroupInfoAttribute(pAd, index, P2pSubelement, P2PSubelementLen); if (bresult == FALSE) goto CleanUp; } #ifdef WFD_SUPPORT if (WfdSubelementLen > 0) { WfdParseSubElmt(pAd, &pP2pEntry->WfdEntryInfo, (PVOID)WfdSubelement, WfdSubelementLen); } #endif /* WFD_SUPPORT */ #ifdef RT_P2P_SPECIFIC_WIRELESS_EVENT if (bSendP2pEvent) P2pSendWirelessEvent(pAd, RT_P2P_DEVICE_FIND, pP2pEntry, Addr2); #endif /* RT_P2P_SPECIFIC_WIRELESS_EVENT */ /* Step 5: Take Some action when the peer is */ /* Decide to connect? or Provision ? or Service discovery ? */ if ((pP2pEntry->P2pClientState <= P2PSTATE_GO_DONE)) { if ((DeviceCap & DEVCAP_INVITE) == DEVCAP_INVITE) P2P_SET_FLAG(pP2pEntry, P2PFLAG_INVITE_ENABLED); if (pP2pEntry->P2pClientState != P2PSTATE_DISCOVERY) { UCHAR ClientState = pP2pEntry->P2pClientState; COPY_MAC_ADDR(&P2pCmd.Addr[0], pP2pEntry->addr); P2pCmd.Idx = index; pP2pEntry->ReTransmitCnt = 0; /*P2pCmd.ConfigMethod = pAd->P2pTable.Client[index].ConfigMethod; */ DBGPRINT(RT_DEBUG_INFO, ("P2p : DevAddr[%02x:%02x:%02x:%02x:%02x:%02x] State = %s.\n", PRINT_MAC(DevAddr), decodeP2PClientState(pP2pEntry->P2pClientState))); DBGPRINT(RT_DEBUG_INFO, ("P2p : GrpCap=%x. DevCap=%x. ConfigMethod= %x.\n", GroupCap, DeviceCap, pP2pEntry->ConfigMethod)); MlmeEnqueue(pAd, P2P_GO_FORM_STATE_MACHINE, P2P_START_COMMUNICATE_CMD_EVT, sizeof(P2P_CMD_STRUCT), &P2pCmd, ClientState); RTMP_MLME_HANDLER(pAd); goto CleanUp; } if (pAd->P2pCfg.ConnectingIndex < MAX_P2P_GROUP_SIZE) { if (MAC_ADDR_EQUAL(pAd->P2pTable.Client[index].addr, &pAd->P2pCfg.ConnectingMAC[0])) { DBGPRINT(RT_DEBUG_INFO, (" From : %02x:%02x:%02x:%02x:%02x:%02x, state = %s\n", PRINT_MAC(Addr2), decodeP2PClientState(pP2pEntry->P2pClientState))); DBGPRINT(RT_DEBUG_INFO, (" P2PTab[%d] Addr : %02x:%02x:%02x:%02x:%02x:%02x\n", index, PRINT_MAC(pP2pEntry->addr))); DBGPRINT(RT_DEBUG_INFO, (" ConenctIdx = %d, Addr : %02x:%02x:%02x:%02x:%02x:%02x\n", pAd->P2pCfg.ConnectingIndex, PRINT_MAC(pAd->P2pCfg.ConnectingMAC))); P2pConnectAfterScan(pAd, bBeacon, index); } } } } } CleanUp: if (P2pSubelement) os_free_mem(pAd, P2pSubelement); #ifdef WFD_SUPPORT if (WfdSubelement) os_free_mem(pAd, WfdSubelement); #endif /* WFD_SUPPORT */ } /* ========================================================================== Description: Processing the Beacon frame when operating as a P2P client. Only called from STA's state machine that is in idle. this function can support NoA and show Ralink IP. Parameters: Note: ========================================================================== */ VOID PeerP2pBeacon( IN PRTMP_ADAPTER pAd, IN PUCHAR pAddr2, IN MLME_QUEUE_ELEM *Elem, IN LARGE_INTEGER TimeStamp) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; UCHAR Addr2[6], SsidLen; UCHAR Ssid[32]; ULONG Peerip; ULONG P2PSubelementLen; PUCHAR P2pSubelement = NULL; PFRAME_802_11 pFrame; /*UCHAR bitmap;*/ PMAC_TABLE_ENTRY pMacEntry = NULL; pFrame = (PFRAME_802_11)Elem->Msg; /* Only check beacon . */ if (pFrame->Hdr.FC.SubType == SUBTYPE_PROBE_RSP) return; if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE) return; pMacEntry = &pAd->MacTab.Content[Elem->Wcid]; if (!(IS_P2P_CLI_ENTRY(pMacEntry) && IS_ENTRY_APCLI(pMacEntry))) { DBGPRINT(RT_DEBUG_INFO, ("1 PeerP2pBeaconProbeRspAtScan failed : wcid = %d. not ValidAsP2P. Bug!please check. \n", Elem->Wcid)); DBGPRINT(RT_DEBUG_INFO, ("pMacEntry Addr = %02x:%02x:%02x:%02x:%02x:%02x.\n", PRINT_MAC(pMacEntry->Addr))); return; } if ((pMacEntry->WpaState != AS_PTKINITDONE)) return; /* Init P2pSubelement */ os_alloc_mem(pAd, &P2pSubelement, MAX_VIE_LEN); if (P2pSubelement == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("3 PeerP2pBeaconProbeRspAtScan::1Allocate memory size(=1024) failed\n")); goto CleanUp; } if (PeerP2pBeaconSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen, &Peerip, &P2PSubelementLen, P2pSubelement)) { /* Parse the power managemenr parameters in here. */ pP2PCtrl->GONoASchedule.LastBeaconTimeStamp = TimeStamp.u.LowPart; P2pParseNoASubElmt(pAd, P2pSubelement, P2PSubelementLen, Elem->Wcid, pFrame->Hdr.Sequence); /* Since we get beacon, check if GO enable and OppPS. */ if (P2P_TEST_BIT(pAd->P2pCfg.CTWindows, P2P_OPPS_BIT)) { pAd->P2pCfg.bKeepSlient = FALSE; /* TO DO : sync with windows if necessary */ /*RTMPDeQueueNoAMgmtPacket(pAd);*/ /* Dequeue outgoing frames from TxSwQueue0..3 queue and process it */ RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS); if (((pAd->P2pCfg.CTWindows&0x7f) > 0) && ((pAd->P2pCfg.CTWindows&0x7f) < 80)) { DBGPRINT(RT_DEBUG_INFO, ("%s:: set P2P CTWindows timer.\n", __FUNCTION__)); RTMPSetTimer(&pAd->P2pCfg.P2pCTWindowTimer, (pAd->P2pCfg.CTWindows&0x7f)); } } } CleanUp: if (P2pSubelement) os_free_mem(NULL, P2pSubelement); } /* ========================================================================== Description: Processing the Beacon frame when operating as a P2P client. Only called from STA's state machine that is in idle. Parameters: *pPeerip : Go's ip. *pMemberip : Other member's ip that is in the same P2P Group. Note: ========================================================================== */ BOOLEAN PeerBeaconParseRalinkIE( IN PRTMP_ADAPTER pAd, IN VOID *Msg, IN ULONG MsgLen, OUT RALINKIP_IE *pRalinkIE, OUT RALINKMBRIP_ELEM *pMemberip, OUT ULONG *pPeerip) { PFRAME_802_11 pFrame; PEID_STRUCT pEid; ULONG Length = 0; BOOLEAN brc = FALSE; PUCHAR Ptr; pFrame = (PFRAME_802_11)Msg; Length += LENGTH_802_11; *pPeerip = 0; NdisZeroMemory(pRalinkIE, sizeof(RALINKIP_IE)); NdisZeroMemory(pMemberip, sizeof(RALINKMBRIP_ELEM)); Ptr = pFrame->Octet; /* get timestamp from payload and advance the pointer */ Ptr += TIMESTAMP_LEN; Length += TIMESTAMP_LEN; /* get beacon interval from payload and advance the pointer */ Ptr += 2; Length += 2; /* get capability info from payload and advance the pointer */ Ptr += 2; Length += 2; pEid = (PEID_STRUCT) Ptr; /* get variable fields from payload and advance the pointer */ while ((Length + 2 + pEid->Len) <= MsgLen) { switch(pEid->Eid) { case IE_VENDOR_SPECIFIC: if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 5)) { RTMPMoveMemory(pRalinkIE, &pEid->Eid, sizeof(RALINKIP_IE)); brc = TRUE; } else if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 8)) { if (pEid->Octet[3] == RALINKOUIMODE_IPRSP) { *pPeerip = *(PULONG)&pEid->Octet[4]; brc = TRUE; } } else if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3)) { /* Beacon broadcast's other peer's ip in Probe rsp. */ if (pEid->Octet[3] == RALINKOUIMODE_MBRIPRSP) { RTMPMoveMemory(pMemberip, &pEid->Octet[4], sizeof(RALINKMBRIP_ELEM)); brc = TRUE; } } break; default: break; } Length = Length + 2 + pEid->Len; /* Eid[1] + Len[1]+ content[Len] */ pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len); } return brc; } /* ========================================================================== Description: Processing Probe Request frame when operating as a P2P Device. Can be called from both as AP's state machine or as STA( that is doing P2P search)'s state machine Parameters: Note: ========================================================================== */ VOID PeerP2pProbeReq( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem) /* IN BOOLEAN bSendRsp) */ { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; ULONG P2PSubelementLen, WpsIELen; UCHAR *P2pSubelement; UCHAR *WpsIE; NDIS_STATUS NStatus; PUCHAR pOutBuffer = NULL; ULONG FrameLen = 0; UCHAR Addr2[MAC_ADDR_LEN]; UCHAR Ssid[MAX_LEN_OF_SSID], SsidLen; UCHAR index, perstindex; /*BOOLEAN bNewlyAdd = FALSE; */ ULONG Peerip; /*RALINKIP_IE RalinkIp;*/ BOOLEAN bresult = FALSE; UCHAR GroupCap, DeviceCap, DevAddr[MAC_ADDR_LEN], StatusCode; BOOLEAN bSendRsp = TRUE; UCHAR Channel = Elem->Priv; #ifdef WFD_SUPPORT ULONG WfdSubelementLen = 0; PUCHAR WfdSubelement = NULL; #endif /* WFD_SUPPORT */ /* When I am a P2P Client , can't send probe response. */ if (P2P_CLI_ON(pAd)) return; if ( pP2PCtrl->bSentProbeRSP != TRUE ) return; /* When I am not in listen channel , can't send probe response. */ if (Channel != pAd->P2pCfg.ListenChannel) { bSendRsp = FALSE; if (P2P_GO_ON(pAd) && (Channel == pAd->P2pCfg.GroupOpChannel)) bSendRsp = TRUE; } P2pSubelement = NULL; WpsIE = NULL; os_alloc_mem(pAd, &P2pSubelement, MAX_VIE_LEN); os_alloc_mem(pAd, &WpsIE, MAX_VIE_LEN); if ((P2pSubelement == NULL) || (WpsIE == NULL)) { DBGPRINT(RT_DEBUG_ERROR, ("%s::Allocate memory size(=%d) failed\n", __FUNCTION__, MAX_VIE_LEN)); goto CleanUp; } #ifdef WFD_SUPPORT os_alloc_mem(pAd, &WfdSubelement, MAX_VIE_LEN); if (WfdSubelement == NULL) { DBGPRINT(RT_DEBUG_ERROR, ("%s::Allocate memory size(=%d) failed\n", __FUNCTION__, MAX_VIE_LEN)); goto CleanUp; } #endif /* WFD_SUPPORT */ if (PeerP2pProbeReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen, &Peerip, &P2PSubelementLen, P2pSubelement, #ifdef WFD_SUPPORT &WfdSubelementLen, WfdSubelement, #endif /* WFD_SUPPORT */ &WpsIELen, WpsIE)) { if (FALSE == P2PDeviceMatch(pAd, Addr2, NULL, 0)) goto CleanUp; DBGPRINT(RT_DEBUG_TRACE, ("P2P Peer Probe Req from %02x %02x %02x %02x %02x %02x \n", PRINT_MAC(Addr2))); index = P2pGroupTabSearch(pAd, Addr2); //QQ TBD, p2p widi need insert p2p entry in probe req. due to p2p0 only in listen state if (index < MAX_P2P_GROUP_SIZE) { DBGPRINT(RT_DEBUG_INFO, ("%s / %s\n", decodeP2PClientState(pAd->P2pTable.Client[index].P2pClientState), decodeP2PState(pAd->P2pCfg.P2PConnectState))); if ((pAd->P2pTable.Client[index].P2pClientState > P2PSTATE_DISCOVERY_UNKNOWN) || (pAd->P2pCfg.P2PConnectState != P2P_CONNECT_IDLE)) bSendRsp = TRUE; } DBGPRINT(RT_DEBUG_TRACE, ("(%d) P2P Peer Probe Req from %02x %02x %02x %02x %02x %02x \n", bSendRsp, PRINT_MAC(Addr2))); if (bSendRsp == TRUE) { /* allocate and send out ProbeRsp frame */ NStatus = MlmeAllocateMemory(pAd, &pOutBuffer); /* Get an unused nonpaged memory */ if (NStatus != NDIS_STATUS_SUCCESS) goto CleanUp; P2PMakeProbe(pAd, Elem, Channel, SUBTYPE_PROBE_RSP, pOutBuffer, &FrameLen); if (FrameLen > 0) MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen); DBGPRINT(RT_DEBUG_INFO, ("Got P2P Peer %d Probe Req . Send probe response back len=%ld \n", index, FrameLen)); MlmeFreeMemory(pAd, pOutBuffer); } /* Check already in table ? */ /* If currently there is no activated P2P profile, we can still check if there is matching peer that is */ /* in Persistent table send probe response. If there is one, maybe we can try to connect to it. */ if (index < MAX_P2P_GROUP_SIZE) { P2pParseSubElmt(pAd, P2pSubelement, P2PSubelementLen, FALSE, NULL, &pAd->P2pTable.Client[index].GroupCapability, &pAd->P2pTable.Client[index].DevCapability, NULL, NULL, DevAddr, NULL, NULL, &SsidLen, Ssid, NULL, NULL, pAd->P2pTable.Client[index].PrimaryDevType, &pAd->P2pTable.Client[index].ListenChannel, &pAd->P2pTable.Client[index].OpChannel, NULL, &pAd->P2pTable.Client[index].GoIntent, &StatusCode, NULL, #ifdef WFD_SUPPORT NULL, NULL, #endif /* WFD_SUPPORT */ NULL); #ifdef WFD_SUPPORT if (WfdSubelementLen > 0) { WfdParseSubElmt(pAd, &pAd->P2pTable.Client[index].WfdEntryInfo, (PVOID)WfdSubelement, WfdSubelementLen); } #endif /* WFD_SUPPORT */ if (pAd->P2pTable.Client[index].P2pClientState == P2PSTATE_CONNECT_COMMAND) { P2P_CMD_STRUCT P2pCmd; UCHAR ClientState = pAd->P2pTable.Client[index].P2pClientState; DBGPRINT(RT_DEBUG_ERROR, ("<< %s: Start Group Form!\n", __FUNCTION__)); COPY_MAC_ADDR(&P2pCmd.Addr[0], pAd->P2pTable.Client[index].addr); P2pCmd.Idx = index; /*P2pStartGroupForm(pAd, pAd->P2pTable.Client[index].addr, index); */ } else if (pAd->P2pTable.Client[index].P2pClientState == P2PSTATE_INVITE_COMMAND) { DBGPRINT(RT_DEBUG_ERROR, ("<< %s: Invite!\n", __FUNCTION__)); if (IS_P2P_CONNECT_IDLE(pAd)) { perstindex = P2pPerstTabSearch(pAd, pAd->P2pTable.Client[index].addr, pAd->P2pTable.Client[index].bssid, pAd->P2pTable.Client[index].InterfaceAddr); bresult = P2pInvite(pAd, pAd->P2pTable.Client[index].addr, perstindex, index); } else if (P2P_GO_ON(pAd)) { /* Invite Case 1 */ bresult = P2pInvite(pAd, pAd->P2pTable.Client[index].addr, MAX_P2P_TABLE_SIZE, index); } else if (P2P_CLI_ON(pAd)) { /* Invite Case 1 */ bresult = P2pInvite(pAd, pAd->P2pTable.Client[index].addr, MAX_P2P_TABLE_SIZE, index); } if (bresult == TRUE) { P2pStopScan(pAd); pP2PCtrl->P2PConnectState = P2P_INVITE; } } } } CleanUp: if (P2pSubelement) os_free_mem(NULL, P2pSubelement); if (WpsIE) os_free_mem(NULL, WpsIE); #ifdef WFD_SUPPORT if (WfdSubelement) os_free_mem(pAd, WfdSubelement); #endif /* WFD_SUPPORT */ } /* ========================================================================== Description: Call this function when receiving WPS EAP Nack frame. Most of time is because incorrect PIN. Parameters: Note: ========================================================================== */ VOID P2pReceiveEapNack( IN PRTMP_ADAPTER pAd, IN PMLME_QUEUE_ELEM pElem) { } VOID P2pMakeProbeRspWSCIE( IN PRTMP_ADAPTER pAd, OUT PUCHAR pOutBuf, OUT PULONG pIeLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; UCHAR WscIEFixed[] = {0xdd, 0x0e, 0x00, 0x50, 0xf2, 0x04}; /* length will modify later */ ULONG Len; PUCHAR pData; PWSC_DEV_INFO pDevInfo; INT templen; /* Role play, Enrollee or Registrar */ pDevInfo = &pP2PCtrl->DevInfo; pData = pOutBuf; Len = 0; *pIeLen = 0; /* 0. WSC fixed IE */ RTMPMoveMemory(pData, &WscIEFixed[0], 6); pData += 6; Len += 6; /* 1. Version */ *((PUSHORT) pData) = cpu2be16(WSC_ID_VERSION); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); *(pData + 4) = pDevInfo->Version; pData += 5; Len += 5; /* 2. Wi-Fi Protected Setup State */ *((PUSHORT) pData) = cpu2be16(WSC_ID_SC_STATE); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); *(pData + 4) = (pAd->P2pCfg.bConfiguredAP ? WSC_SCSTATE_CONFIGURED : WSC_SCSTATE_UNCONFIGURED); pData += 5; Len += 5; /* 3. Response Type WSC_ID_RESP_TYPE */ *((PUSHORT) pData) = cpu2be16(WSC_ID_RESP_TYPE); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); if (P2P_GO_ON(pAd)) *(pData + 4) = WSC_MSGTYPE_AP_WLAN_MGR; else *(pData + 4) = WSC_MSGTYPE_ENROLLEE_INFO_ONLY; pData += 5; Len += 5; /* 4. UUID-E */ *((PUSHORT) pData) = cpu2be16(WSC_ID_UUID_E); *((PUSHORT) (pData + 2)) = cpu2be16(0x0010); NdisMoveMemory((pData + 4), pDevInfo->Uuid, 16); pData += 20; Len += 20; // 5. Manufacture templen = AppendWSCTLV(WSC_ID_MANUFACTURER, pData, pAd->P2pCfg.Manufacturer, pAd->P2pCfg.ManufacturerLen); pData += templen; Len += templen; /* We don't support full length manufacture, model name, model name and model serial */ /* because it shall overflow. (> 255 bytes) */ /* 6. Model Name */ templen = AppendWSCTLV(WSC_ID_MODEL_NAME, pData, pAd->P2pCfg.ModelName, pAd->P2pCfg.ModelNameLen); pData += templen; Len += templen; /* 7. Model Number */ templen = AppendWSCTLV(WSC_ID_MODEL_NUMBER, pData, pAd->P2pCfg.ModelNumber, pAd->P2pCfg.ModelNumberLen); pData += templen; Len += templen; /* 8. Serial Number */ templen = AppendWSCTLV(WSC_ID_SERIAL_NUM, pData, pAd->P2pCfg.SerialNumber, pAd->P2pCfg.SerialNumberLen); pData += templen; Len += templen; /* 9. Primary device type */ *((PUSHORT) pData) = cpu2be16(WSC_ID_PRIM_DEV_TYPE); *((PUSHORT) (pData + 2)) = cpu2be16(0x0008); NdisMoveMemory((pData + 4), pDevInfo->PriDeviceType, 8); pData += 12; Len += 12; /* 10. Primary device name */ *((PUSHORT) pData) = cpu2be16(WSC_ID_DEVICE_NAME); *((PUSHORT) (pData + 2)) = cpu2be16(pAd->P2pCfg.DeviceNameLen); NdisMoveMemory((pData + 4), pAd->P2pCfg.DeviceName, pAd->P2pCfg.DeviceNameLen); pData += pAd->P2pCfg.DeviceNameLen + 4; Len += pAd->P2pCfg.DeviceNameLen + 4; /* 11. Config Method */ *((PUSHORT) pData) = cpu2be16(WSC_ID_CONFIG_METHODS); *((PUSHORT) (pData + 2)) = cpu2be16(0x0002); *((PUSHORT) (pData + 4)) = cpu2be16(0x0188); pData += 6; Len += 6; /* The WPS IE shall contain the attributes required for an AP/Registrar as described in Ħħ7.2.5 of [6] */ { /* 12. RF band, shall change based on current channel */ *((PUSHORT) pData) = cpu2be16(WSC_ID_RF_BAND); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); *(pData + 4) = pDevInfo->RfBand; pData += 5; Len += 5; /* 13. DPID shall be a required attribute if Credentials are available and ready for immediate use. */ if (pAd->P2pCfg.Dpid != DEV_PASS_ID_NOSPEC) { /* Device Password ID */ *((PUSHORT) pData) = cpu2be16(WSC_ID_DEVICE_PWD_ID); *((PUSHORT) (pData + 2)) = cpu2be16(0x0002); *((PUSHORT) (pData + 4)) = cpu2be16(pAd->P2pCfg.Dpid); pData += 6; Len += 6; } /* When PBC has triggered, done or connected with timeout, */ /* we must change the value of SelReg. */ #ifdef CONFIG_AP_SUPPORT if ((pAd->ApCfg.MBSSID[BSS0].WscControl.WscSelReg) && (pAd->P2pCfg.Dpid == DEV_PASS_ID_PBC)) { if (P2P_GO_ON(pAd)) { /* 14. Selected Registrar */ *((PUSHORT) pData) = cpu2be16(WSC_ID_SEL_REGISTRAR); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); *(pData + 4) = pAd->ApCfg.MBSSID[BSS0].WscControl.WscSelReg; /* AP */ pData += 5; Len += 5; /* Device Password ID (append it above) */ /* 15. Selected Registrar Config Methods */ *((PUSHORT) pData) = cpu2be16(WSC_ID_SEL_REG_CFG_METHODS); *((PUSHORT) (pData + 2)) = cpu2be16(0x0002); *((PUSHORT) (pData + 4)) = cpu2be16(0x18c); /* Support All : PBC, Keypad, Label, Display */ pData += 6; Len += 6; DBGPRINT(RT_DEBUG_INFO, ("P2pMakeProbeRspWSCIE: SelReg=1 \n")); } } #endif /* CONFIG_AP_SUPPORT */ } /* update the total length in vendor specific IE */ *(pOutBuf+1) = Len - 2; /* fill in output buffer */ *pIeLen = Len; } /* ======================================================================== Routine Description: Make WSC IE for the ProbeReq frame for P2P Spec requirement Arguments: pAdapter - NIC Adapter pointer pOutBuf - all of WSC IE field pIeLen - length Return Value: None IRQL = DISPATCH_LEVEL Note: None ======================================================================== */ VOID P2pMakeProbeReqIE( IN PRTMP_ADAPTER pAd, OUT PUCHAR pOutBuf, OUT PUCHAR pIeLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; UCHAR WscIEFixed[] = {0xdd, 0x0e, 0x00, 0x50, 0xf2, 0x04}; /* length will modify later */ UCHAR Len; PUCHAR pData; PWSC_REG_DATA pReg; PWSC_DEV_INFO pDevInfo; UCHAR OutMsgBuf[512]; pReg = (PWSC_REG_DATA) &pAd->StaCfg.WscControl.RegData; /* Role play, Enrollee or Registrar */ pDevInfo = (PWSC_DEV_INFO) &pReg->SelfInfo; pData = (PUCHAR) &OutMsgBuf[0]; Len = 0; *pIeLen = 0; /* 0. WSC fixed IE */ RTMPMoveMemory(pData, &WscIEFixed[0], 6); pData += 6; Len += 6; /* 1. Version */ *((PUSHORT) pData) = cpu2be16(WSC_ID_VERSION); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); *(pData + 4) = pDevInfo->Version; pData += 5; Len += 5; /* 2. Request Type */ *((PUSHORT) pData) = cpu2be16(WSC_ID_REQ_TYPE); *((PUSHORT) (pData + 2)) = cpu2be16(0x0001); *(pData + 4) = ((pAd->StaCfg.WscControl.WscConfMode == WSC_REGISTRAR) ? WSC_MSGTYPE_AP_WLAN_MGR : WSC_MSGTYPE_ENROLLEE_OPEN_8021X ); pData += 5; Len += 5; /* 3. Config method */ *((PUSHORT) pData) = cpu2be16(WSC_ID_CONFIG_METHODS); *((PUSHORT) (pData + 2)) = cpu2be16(0x0002); *((PUSHORT) (pData + 4)) = cpu2be16(0x188);/* Keypad, Display, PBC */ pData += 6; Len += 6; /* 4. UUID-(E or R) */ *((PUSHORT) pData) = ((pAd->StaCfg.WscControl.WscConfMode == WSC_REGISTRAR) ? cpu2be16(WSC_ID_UUID_R) : cpu2be16(WSC_ID_UUID_E)); *((PUSHORT) (pData + 2)) = cpu2be16(0x0010); NdisMoveMemory((pData + 4), pDevInfo->Uuid, 16); pData += 20; Len += 20; /* 5. Primary device type */ *((PUSHORT) pData) = cpu2be16(WSC_ID_PRIM_DEV_TYPE); *((PUSHORT) (pData + 2)) = cpu2be16(0x0008); NdisMoveMemory((pData + 4), pP2PCtrl->DevInfo.PriDeviceType, 8); pData += 12; Len += 12; /* 6. Primary device name */ *((PUSHORT) pData) = cpu2be16(WSC_ID_DEVICE_NAME); *((PUSHORT) (pData + 2)) = cpu2be16(pP2PCtrl->DeviceNameLen); NdisMoveMemory((pData + 4), pP2PCtrl->DeviceName, pP2PCtrl->DeviceNameLen); pData += (pP2PCtrl->DeviceNameLen + 4); Len += (pP2PCtrl->DeviceNameLen + 4); /* 13. DPID shall be a required attribute if Credentials are available and ready for immediate use. */ { USHORT Dpid = 0x188; /* Device Password ID */ *((PUSHORT) pData) = cpu2be16(WSC_ID_DEVICE_PWD_ID); *((PUSHORT) (pData + 2)) = cpu2be16(0x0002); *((PUSHORT) (pData + 4)) = cpu2be16(Dpid); pData += 6; Len += 6; } /* update the total length in vendor specific IE */ OutMsgBuf[1] = Len - 2; /* fill in output buffer */ *pIeLen = Len; NdisMoveMemory(pOutBuf, &OutMsgBuf[0], *pIeLen); } /* ========================================================================== Description: Prepare Probe reqeust or response frame when opeartin as P2P DEvice. Parameters: pDest : buffer to put frame content. pFrameLen : frame length. Note: ========================================================================== */ VOID P2PMakeProbe( IN PRTMP_ADAPTER pAd, IN MLME_QUEUE_ELEM *Elem, IN UCHAR DsChannel, IN USHORT SubType, OUT PUCHAR pDest, OUT ULONG *pFrameLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; PUCHAR pOutBuffer = pDest; UCHAR Addr2[6]; PHEADER_802_11 pHeader; HEADER_802_11 ProbeRspHdr; UCHAR RSNIe=IE_WPA2;/*, RSNIe2=IE_WPA2, RSN_Len=22; */ UCHAR tmpSupRate[MAX_LEN_OF_SUPPORTED_RATES]; UCHAR tmpExtRateLen; LARGE_INTEGER FakeTimestamp; UCHAR DsLen = 1, SsidLen = 0; ULONG TmpLen; ULONG FrameLen = 0; /*UCHAR ErpIeLen = 1; */ /*UCHAR P2pIEFixed[6] = {0xdd, 0x08, 0x00, 0x50, 0xf2, 0x09}; */ /* length will modify later */ USHORT CapabilityInfo; UCHAR SupRateLen; PUCHAR ptr; pHeader = (PHEADER_802_11) &Elem->Msg[0]; RTMPMoveMemory(Addr2, pHeader->Addr2, 6); if (SubType== SUBTYPE_PROBE_RSP) { DBGPRINT(RT_DEBUG_INFO, ("SYNC - P2PMakeProbeRsp. Addr2 = %02x:%02x:%02x:%02x:%02x:%02x...\n", PRINT_MAC(Addr2))); MgtMacHeaderInit(pAd,&ProbeRspHdr, SUBTYPE_PROBE_RSP, 0, Addr2, #ifdef P2P_SUPPORT pP2PCtrl->CurrentAddress, #endif /* P2P_SUPPORT */ pP2PCtrl->CurrentAddress); } else MgtMacHeaderInit(pAd, &ProbeRspHdr, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, #ifdef P2P_SUPPORT pP2PCtrl->CurrentAddress, #endif /* P2P_SUPPORT */ BROADCAST_ADDR); NdisMoveMemory(tmpSupRate,pAd->CommonCfg.SupRate,pAd->CommonCfg.SupRateLen); tmpExtRateLen = pAd->CommonCfg.ExtRateLen; /* P2P device's probe response need to set both ess and ibss bit to zero. */ CapabilityInfo = CAP_GENERATE(0, 0, 1, 0, 0, 0); if (P2P_GO_ON(pAd)) CapabilityInfo = CAP_GENERATE(1, 0, 1, 0, 0, 0); tmpSupRate[0] = 0x8C; /* 6 mbps, in units of 0.5 Mbps, basic rate */ tmpSupRate[1] = 0x12; /* 9 mbps, in units of 0.5 Mbps */ tmpSupRate[2] = 0x98; /* 12 mbps, in units of 0.5 Mbps, basic rate */ tmpSupRate[3] = 0x24; /* 18 mbps, in units of 0.5 Mbps */ tmpSupRate[4] = 0xb0; /* 24 mbps, in units of 0.5 Mbps, basic rate */ tmpSupRate[5] = 0x48; /* 36 mbps, in units of 0.5 Mbps */ tmpSupRate[6] = 0x60; /* 48 mbps, in units of 0.5 Mbps */ tmpSupRate[7] = 0x6c; /* 54 mbps, in units of 0.5 Mbps */ SupRateLen = 8; tmpExtRateLen = pAd->CommonCfg.ExtRateLen; if (DsChannel == 0) DBGPRINT(RT_DEBUG_ERROR, ("SYNC - P2PMakeProbeRsp. DsChannel = is 0 !!!!!\n")); if (SubType == SUBTYPE_PROBE_RSP) { SsidLen = WILDP2PSSIDLEN; if (P2P_GO_ON(pAd)) SsidLen = pP2PCtrl->SSIDLen; MakeOutgoingFrame(pOutBuffer, &FrameLen, sizeof(HEADER_802_11), &ProbeRspHdr, TIMESTAMP_LEN, &FakeTimestamp, 2, &pAd->CommonCfg.BeaconPeriod, 2, &CapabilityInfo, 1, &SsidIe, 1, &SsidLen, SsidLen, pP2PCtrl->SSID, 1, &SupRateIe, 1, &SupRateLen, SupRateLen, tmpSupRate, END_OF_ARGS); } else { SsidLen = WILDP2PSSIDLEN; MakeOutgoingFrame(pOutBuffer, &FrameLen, sizeof(HEADER_802_11), &ProbeRspHdr, 1, &SsidIe, 1, &SsidLen, SsidLen, &WILDP2PSSID[0], 1, &SupRateIe, 1, &SupRateLen, SupRateLen, tmpSupRate, END_OF_ARGS); } if ((DsChannel > 0) && (DsChannel <= 14)) { MakeOutgoingFrame(pOutBuffer+FrameLen, &TmpLen, 1, &DsIe, 1, &DsLen, 1, &DsChannel, END_OF_ARGS); FrameLen += TmpLen; } /* Add this IE after I already become GO. */ if (tmpExtRateLen && (P2P_GO_ON(pAd))) { } /* Msut append RSN_IE because P2P uses WPA2PSK. */ { MakeOutgoingFrame(pOutBuffer + FrameLen, &TmpLen, 1, &RSNIe, 1, &pAd->StaCfg.RSNIE_Len, pAd->StaCfg.RSNIE_Len, pAd->StaCfg.RSN_IE, END_OF_ARGS); FrameLen += TmpLen; } /* // add Ralink-specific IE here - Byte0.b0=1 for aggregation, Byte0.b1=1 for piggy-back if (pAd->CommonCfg.bAggregationCapable) { if ((pAd->CommonCfg.bPiggyBackCapable)) { UCHAR RalinkSpecificIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x07, 0x00, 0x00, 0x00}; MakeOutgoingFrame(pOutBuffer+FrameLen, &TmpLen, 9, RalinkSpecificIe, END_OF_ARGS); FrameLen += TmpLen; } else { UCHAR RalinkSpecificIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x05, 0x00, 0x00, 0x00}; MakeOutgoingFrame(pOutBuffer+FrameLen, &TmpLen, 9, RalinkSpecificIe, END_OF_ARGS); FrameLen += TmpLen; } } else { UCHAR RalinkSpecificIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x06, 0x00, 0x00, 0x00}; MakeOutgoingFrame(pOutBuffer+FrameLen, &TmpLen, 9, RalinkSpecificIe, END_OF_ARGS); FrameLen += TmpLen; } if ((pP2PCtrl->P2pPhyMode != P2P_PHYMODE_LEGACY_ONLY)) { UCHAR HtLen, AddHtLen, NewExtLen; ADD_HT_INFO_IE AddHTInfo; // Useful as AP. HT_CAPABILITY_IE HtCapability; NdisMoveMemory(&HtCapability, &pAd->ApCfg.HtCapability, sizeof(HT_CAPABILITY_IE)); if (pAd->Antenna.field.RxPath > 1) { HtCapability.MCSSet[1] = 0xff; } else { HtCapability.MCSSet[1] = 0x00; } HtLen = sizeof(pAd->ApCfg.HtCapability); AddHtLen = sizeof(pAd->ApCfg.AddHTInfoIe); NewExtLen = 1; NdisMoveMemory(&AddHTInfo, &pAd->ApCfg.AddHTInfoIe, sizeof(ADD_HT_INFO_IE)); //New extension channel offset IE is included in Beacon, Probe Rsp or channel Switch Announcement Frame MakeOutgoingFrame(pOutBuffer + FrameLen, &TmpLen, 1, &HtCapIe, 1, &HtLen, sizeof(HT_CAPABILITY_IE), &HtCapability, 1, &AddHtInfoIe, 1, &AddHtLen, sizeof(ADD_HT_INFO_IE), &AddHTInfo, 1, &NewExtChanIe, 1, &NewExtLen, sizeof(NEW_EXT_CHAN_IE), &pAd->CommonCfg.NewExtChanOffset, END_OF_ARGS); FrameLen += TmpLen; } */ /* New extension channel offset IE is included in Beacon, Probe Rsp or channel Switch Announcement Frame */ P2pMakeProbeRspWSCIE(pAd, pOutBuffer + FrameLen, &TmpLen); FrameLen += TmpLen; ptr = pOutBuffer + FrameLen; P2pMakeP2pIE(pAd, (UCHAR)SubType, ptr, &TmpLen); FrameLen += TmpLen; #ifdef WFD_SUPPORT { ULONG WfdIeBitmap; ptr = pOutBuffer + FrameLen; WfdIeBitmap = (0x1 << SUBID_WFD_DEVICE_INFO) | (0x1 << SUBID_WFD_ASSOCIATED_BSSID) | (0x1 << SUBID_WFD_COUPLED_SINK_INFO); if (SubType == SUBTYPE_PROBE_RSP) WfdIeBitmap |= (0x1 << SUBID_WFD_SESSION_INFO) | (0x1 << SUBID_WFD_ALTERNATE_MAC_ADDR); WfdMakeWfdIE(pAd, WfdIeBitmap, ptr, &TmpLen); FrameLen += TmpLen; } #endif /* WFD_SUPPORT */ *pFrameLen = FrameLen; } /* ========================================================================== Description: Make P2P IE. Parameters: reutrn IE lenght and buffer. Note: ========================================================================== */ VOID P2pMakeP2pIE( IN PRTMP_ADAPTER pAd, IN UCHAR PacketType, OUT PUCHAR pOutBuf, OUT PULONG pIeLen) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; UCHAR P2pIEFixed[6] = {0xdd, 0x0e, 0x00, 0x50, 0xf2, 0x09}; /* length will modify later */ ULONG Len; PUCHAR pData; ULONG TempLen; UCHAR Status; /*UCHAR Maganed; */ UCHAR P2pCapability[2]; PUCHAR pBuf; RTMPMoveMemory(&P2pIEFixed[2], P2POUIBYTE, 4); pData = pOutBuf; Len = 0; *pIeLen = 0; /* 0. P2P fixed IE */ RTMPMoveMemory(pData, &P2pIEFixed[0], 6); pData += 6; Len += 6; P2pCapability[0] = pAd->P2pCfg.P2pCapability[0]; P2pCapability[1] = pAd->P2pCfg.P2pCapability[1]; /* : . */ if (PacketType == SUBTYPE_ASSOC_RSP) { Status = P2PSTATUS_SUCCESS; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_STATUS, &Status, pData); Len += TempLen; pData += TempLen; if (IS_P2P_SUPPORT_EXT_LISTEN(pAd)) { DBGPRINT(RT_DEBUG_TRACE, ("P2pMakeP2pIE (PacketType = %d) insert SUBID_P2P_EXT_LISTEN_TIMING .\n", PacketType)); TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_EXT_LISTEN_TIMING, NULL, pData); Len += TempLen; pData += TempLen; } /* this is managed infra STA connect to Infra AP. So Add P2P Interface. then this's all. return here. */ DBGPRINT(RT_DEBUG_TRACE, ("<----- P2pMakeP2pIE For managed STA. (Len = %ld) \n", Len)); *(pOutBuf+1) = (Len-2); *pIeLen = Len; return; } /* Conenct to Managed AP. */ if ((pP2PCtrl->P2pManagedParm.TotalNumOfP2pAttribute > 0) && (PacketType == SUBTYPE_ASSOC_REQ)) { /* Always support Managed when connecting using Assoc Req. */ P2pCapability[0] |= DEVCAP_INFRA_MANAGED; /* If the cross connect is enabled, check whether we need to turn off it because Managed AP Asks us to do so. */ /* If already turned off, no need to check. */ if ((!P2P_TEST_BIT(P2pCapability[1], GRPCAP_CROSS_CONNECT)) && RTMPEqualMemory(pP2PCtrl->Bssid, pAd->P2pCfg.P2pManagedParm.ManageAPBSsid, MAC_ADDR_LEN)) { DBGPRINT(RT_DEBUG_TRACE, ("pAd->P2pCfg.P2pManagedParm.APP2pManageability = %x \n", pAd->P2pCfg.P2pManagedParm.APP2pManageability)); /* If This is the 1st Managed AP that I just want to connect to, but now I don't turn off the cross connect bit yet. */ /* I have to turn off this bit when connecting to this AP. */ if ((pAd->P2pCfg.P2pManagedParm.APP2pManageability != 0xff) && (P2P_TEST_BIT(pAd->P2pCfg.P2pManagedParm.APP2pManageability, P2PMANAGED_ENABLE_BIT)) && (!P2P_TEST_BIT(pAd->P2pCfg.P2pManagedParm.APP2pManageability, P2PMANAGED_ICS_ENABLE_BIT))) { P2pCapability[1] &= (~GRPCAP_CROSS_CONNECT); DBGPRINT(RT_DEBUG_TRACE, ("Turn off Corss Conenct bit in Assoc Req. %x \n", (~GRPCAP_CROSS_CONNECT))); } } TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CAP, P2pCapability, pData); Len += TempLen; pData += TempLen; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_INTERFACE, pP2PCtrl->CurrentAddress, pData); Len += TempLen; pData += TempLen; /* this is managed infra STA connect to Infra AP. So Add P2P Interface. then this's all. return here. */ DBGPRINT(RT_DEBUG_ERROR, ("<-----P2pMakeP2pIE For managed STA. (Len = %ld PacketType = %d.) \n", Len, PacketType)); *(pOutBuf+1) = (Len-2); *pIeLen = Len; return; } if (IS_CLIENT_DISCOVERY_ON(pAd)) { P2pCapability[0] |= DEVCAP_CLIENT_DISCOVER; } if (PacketType == SUBTYPE_PROBE_REQ) { /* Probe Request Group Capability bit is reserved. (TestPlan 4.1.1) */ P2pCapability[1] = 0; } pBuf = &P2pCapability[0]; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_CAP, pBuf, pData); Len += TempLen; pData += TempLen; /*if ((PortSubtype == PORTSUBTYPE_P2PClient) */ if ((pP2PCtrl->Rule == P2P_IS_CLIENT) || (pP2PCtrl->Rule == P2P_IS_DEVICE) || (PacketType == SUBTYPE_PROBE_REQ)) { /* Doesn't need to specify who I am searching for. So delete */ } else { TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_ID, pP2PCtrl->CurrentAddress, pData); Len += TempLen; pData += TempLen; } if (PacketType == SUBTYPE_PROBE_REQ) { if (INFRA_ON(pAd)) pBuf = &pAd->CommonCfg.Channel; else pBuf = &pP2PCtrl->ListenChannel; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_LISTEN_CHANNEL, pBuf, pData); Len += TempLen; pData += TempLen; /*if (pP2PCtrl->PortSubtype == PORTSUBTYPE_P2PGO) */ if (pP2PCtrl->Rule == P2P_IS_GO) { if (INFRA_ON(pAd)) pBuf = &pAd->CommonCfg.Channel; else pBuf = &pP2PCtrl->GroupChannel; TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_OP_CHANNEL, pBuf, pData); Len += TempLen; pData += TempLen; } } if ((PacketType == SUBTYPE_PROBE_RSP) || (PacketType == SUBTYPE_ASSOC_REQ) || (PacketType == SUBTYPE_REASSOC_REQ)) { if (PacketType == SUBTYPE_ASSOC_REQ) { DBGPRINT(RT_DEBUG_INFO, (" P2pMakeP2pIE (PacketType = %d) insert SUBID_P2P_DEVICE_INFO. DeviceNameLen = %ld.\n", PacketType, pAd->P2pCfg.DeviceNameLen)); } TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_DEVICE_INFO, pP2PCtrl->CurrentAddress, pData); Len += TempLen; pData += TempLen; } if (((PacketType == SUBTYPE_PROBE_RSP) || (PacketType == SUBTYPE_PROBE_REQ) || (PacketType == SUBTYPE_ASSOC_RSP)) && (IS_EXT_LISTEN_ON(pAd))) { DBGPRINT(RT_DEBUG_INFO, ("P2pMakeP2pIE (PacketType = %d) insert SUBID_P2P_EXT_LISTEN_TIMING .\n", PacketType)); TempLen = InsertP2PSubelmtTlv(pAd, SUBID_P2P_EXT_LISTEN_TIMING, NULL, pData); Len += TempLen; pData += TempLen; } if (P2P_GO_ON(pAd) && (PacketType == SUBTYPE_PROBE_RSP)) { /* If I am GO, must insert Group Info in my probe response to Probe Request that has P2P IE. */ TempLen = InsertP2PGroupInfoTlv(pAd, pData); Len += TempLen; pData += TempLen; } /* The NoA has its own P2P IE. So NoA Attribute lenght doesn't count here. */ *(pOutBuf+1) = (Len-2); if (P2P_GO_ON(pAd) && (pAd->P2pCfg.GONoASchedule.bValid == TRUE)) { TempLen = P2pUpdateNoAProbeRsp(pAd, pData); Len += TempLen; pData += TempLen; } else if (P2P_GO_ON(pAd) && (P2P_TEST_BIT(pAd->P2pCfg.CTWindows, P2P_OPPS_BIT))) { TempLen = P2pUpdateNoAProbeRsp(pAd, pData); Len += TempLen; pData += TempLen; } DBGPRINT(RT_DEBUG_INFO, ("<----- P2pMakeP2pIE (Len = %ld) \n", Len)); *pIeLen = Len; } /* ========================================================================== Description: Processing WSC IE and put to OUTPUT buffer. Parameters: Note: ========================================================================== */ VOID P2PParseWPSIE( IN PUCHAR pWpsData, IN USHORT WpsLen, OUT PUSHORT Dpid, OUT PUSHORT ConfigMethod, OUT PUCHAR DeviceName, OUT UCHAR *DeviceNameLen) { USHORT Length; PUCHAR pData; USHORT WscType, WscLen; if (pWpsData == NULL) return; if (DeviceNameLen !=NULL) *DeviceNameLen = 0; pData = pWpsData+4; /*pWpsData+6;*/ Length = WpsLen - 6; if (pData == NULL) return; /* Start to process WSC IEs */ while (Length > 4) { WscType = cpu2be16(*((PUSHORT) pData)); WscLen = cpu2be16(*((PUSHORT) (pData + 2))); pData += 4; Length -= 4; /* Parse M1 WSC type and store to RegData structure */ switch (WscType) { case WSC_ID_DEVICE_NAME: /* 1 */ if (DeviceName !=NULL) { RTMPMoveMemory(DeviceName, pData, 32); if (DeviceNameLen !=NULL) *DeviceNameLen = (UCHAR)WscLen; DBGPRINT(RT_DEBUG_INFO,("%s : DeviceName = %c%c%c%c%c ...\n", __FUNCTION__, DeviceName[0], DeviceName[1], DeviceName[2],DeviceName[3],DeviceName[4])); } break; case WSC_ID_DEVICE_PWD_ID: /* 1 */ if (Dpid !=NULL) { *Dpid = be2cpu16(*((USHORT *) pData)); /**Dpid = *((PUSHORT) pData); */ DBGPRINT(RT_DEBUG_INFO,("%s : Dpid = %s .\n", __FUNCTION__, decodeDpid(*Dpid))); } break; case WSC_ID_CONFIG_METHODS: /* 1 */ if (ConfigMethod !=NULL) { *ConfigMethod = be2cpu16(*((PUSHORT) pData)); DBGPRINT(RT_DEBUG_INFO,(" Config = %x ..\n", *ConfigMethod)); } break; default: /*DBGPRINT(RT_DEBUG_TRACE, ("WscRecvMessageM1 --> Unknown IE 0x%04x\n", WscType)); */ break; } /* Offset to net WSC Ie */ pData += WscLen; Length -= WscLen; } } /* ========================================================================== Description: Go PeerDisassocReq Action. Parameters: Note: ========================================================================== */ VOID GoPeerDisassocReq( IN PRTMP_ADAPTER pAd, IN PUCHAR Addr2) { /* Set to a temporary state. If thie device connect within 25 seconds. he may use WPS to connect. */ P2pGroupTabDelete(pAd, P2P_NOT_FOUND, Addr2); /* Don't Stop GO immediately. Give some time for this client to reconnect with 13 seconds. */ /* when StopGo timer expired, */ } /* ========================================================================== Description: Update P2P beacon frame for P2P IE Group Info Attribute. Parameters: Note: ========================================================================== */ ULONG P2pUpdateGroupBeacon( IN PRTMP_ADAPTER pAd, IN ULONG StartPosition) { return 0; } /* ========================================================================== Description: Update P2P beacon frame for P2P IE NoA Attribute. When I am GO. Parameters: Note: ========================================================================== */ ULONG P2pUpdateNoABeacon( IN PRTMP_ADAPTER pAd, IN UCHAR apidx, /* IN ULONG StartPosition) */ IN PUCHAR pDest) { PRT_P2P_CONFIG pP2PCtrl = &pAd->P2pCfg; /*PUCHAR pDest;*/ UCHAR P2PIEFixed[] = {0xdd, 0x16, 0x00, 0x50, 0xf2, 0x09}; /* length will modify later */ /*UCHAR i;*/ /*PUCHAR ptr;*/ RTMPMoveMemory(&P2PIEFixed[2], P2POUIBYTE, 4); DBGPRINT(RT_DEBUG_INFO, ("<---- %s\n", __FUNCTION__)); if (pP2PCtrl->GONoASchedule.bValid == TRUE) { /*pDest = &pAd->BeaconBuf[StartPosition];*/ /*PUCHAR pDest = (PUCHAR)pAd->ApCfg.MBSSID[apidx].BeaconBuf;*/ /* Always support attach one NoA.. So.. length is fixed to 0x16. :) */ RTMPMoveMemory(pDest, P2PIEFixed, 6); *(pDest+6) = SUBID_P2P_NOA; /* Length is 13*n + 2 = 15 when n = 1 */ *(pDest+7) = 15; /* Lenght 2nd byte */ *(pDest+8) = 0; /* Index. */ *(pDest+9) = pP2PCtrl->GONoASchedule.Token; /* CT Windows and OppPS parm */ *(pDest+10) = pP2PCtrl->CTWindows; /* Count. Test Plan set to 255. */ *(pDest+11) = pP2PCtrl->GONoASchedule.Count; /* Duration */ RTMPMoveMemory((pDest+12), &pP2PCtrl->GONoASchedule.Duration, 4); /* Interval */ RTMPMoveMemory((pDest+16), &pP2PCtrl->GONoASchedule.Interval, 4); RTMPMoveMemory((pDest+20), &pP2PCtrl->GONoASchedule.StartTime, 4); pAd->GOBeaconBufNoALen = 24; } else { } DBGPRINT(RT_DEBUG_INFO, ("----> %s\n", __FUNCTION__)); return pAd->GOBeaconBufNoALen; } /* ========================================================================== Description: Update P2P beacon frame for P2P IE NoA Attribute. When I am GO. Parameters: Note: ========================================================================== */ ULONG P2pUpdateNoAProbeRsp( IN PRTMP_ADAPTER pAd, IN PUCHAR pInbuffer) { PUCHAR pDest; UCHAR P2PIEFixed[6] = {0xdd, 0x16, 0x00, 0x50, 0xf2, 0x09}; /* length will modify later */ /*UCHAR i; */ /*PUCHAR ptr; */ RTMPMoveMemory(&P2PIEFixed[2], P2POUIBYTE, 4); if (pAd->P2pCfg.GONoASchedule.bValid == TRUE) { pDest = pInbuffer; /* Always support attach one NoA.. So.. length is fixed to 0x16. :) */ RTMPMoveMemory(pDest, P2PIEFixed, 6); *(pDest+6) = SUBID_P2P_NOA; /* Length is 13*n + 2 = 15 when n = 1 */ *(pDest+7) = 15; /* Lenght 2nd byte */ *(pDest+8) = 0; /* Index. */ *(pDest+9) = pAd->P2pCfg.GONoASchedule.Token; /* CT Windows and OppPS parm. Don't turn on both. So Set CTWindows = 0 */ *(pDest+10) = 0; /* Count. Test Plan set to 255. */ *(pDest+11) = pAd->P2pCfg.GONoASchedule.Count; /* Duration */ RTMPMoveMemory((pDest+12), &pAd->P2pCfg.GONoASchedule.Duration, 4); /* Interval */ RTMPMoveMemory((pDest+16), &pAd->P2pCfg.GONoASchedule.Interval, 4); RTMPMoveMemory((pDest+20), &pAd->P2pCfg.GONoASchedule.StartTime, 4); return 24; } else if (P2P_TEST_BIT(pAd->P2pCfg.CTWindows, P2P_OPPS_BIT)) { P2PIEFixed[1] = 0x9; pDest = pInbuffer; RTMPMoveMemory(pDest, P2PIEFixed, 6); *(pDest+6) = SUBID_P2P_NOA; /* Length is 13*n + 2 = 15 when n = 1 */ *(pDest+7) = 2; /* Lenght 2nd byte */ *(pDest+8) = 0; /* Index. */ *(pDest+9) = pAd->P2pCfg.GONoASchedule.Token; /* CT Windows and OppPS parm */ *(pDest+10) = pAd->P2pCfg.CTWindows; return 11; } else { return 0; } } /* ========================================================================== Description: Update P2P beacon frame and save to BeaconBuf[]. Parameters: Note: ========================================================================== */ VOID P2pUpdateBssBeacon( IN PRTMP_ADAPTER pAd, IN PUCHAR pCapability, IN PUCHAR pIpReqP2ptabindex) { } VOID GOUpdateBeaconFrame( IN PRTMP_ADAPTER pAd) { } /* ========================================================================== Description: Make P2P beacon frame and save to BeaconBuf[] Parameters: Note: ========================================================================== */ VOID P2pMakeBssBeacon( IN PRTMP_ADAPTER pAd) { } /* ======================================================================== Routine Description: Check REinvoke's invitation Request frame . Arguments: - NIC Adapter pointer Return Value: FALSE - None of channel in ChannelList Match any channel in pAd->ChannelList[] array IRQL = DISPATCH_LEVEL Note: ======================================================================== */ VOID P2pCheckInviteReq( IN PRTMP_ADAPTER pAd, IN BOOLEAN bIAmGO, IN UCHAR index, IN PUCHAR ChannelList, IN PUCHAR BssidAddr, IN UCHAR OpChannel, IN PUCHAR Ssid, IN UCHAR SsidLen, IN UCHAR *pRspStatus) { UCHAR i; *pRspStatus = P2PSTATUS_SUCCESS; /* Check if have Common Channels. */ if (FALSE == P2pCheckChannelList(pAd, ChannelList)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - no common channel = %d...\n", *ChannelList)); *pRspStatus = P2PSTATUS_NO_CHANNEL; return; } /* Invite Req from a CLient doesn't includes group_bssid in the request. So doesn't need check. */ /* Check Bssid is correct. */ if (!RTMPEqualMemory(BssidAddr, pAd->P2pTable.PerstEntry[index].Addr, MAC_ADDR_LEN) && (bIAmGO == FALSE)) { *pRspStatus = P2PSTATUS_INVALID_PARM; DBGPRINT(RT_DEBUG_ERROR, (" P2P - MAc addr invalid .\n")); return; } /* invite Req from Owner include OpChannel. from Client, doesn't include. */ if (bIAmGO == FALSE) { /* Check Oopchannel is correct. */ for (i = 0;i < pAd->ChannelListNum;i++) { if (pAd->ChannelList[i].Channel == OpChannel) { break; } } if ( i == pAd->ChannelListNum) { *pRspStatus = P2PSTATUS_NO_CHANNEL; DBGPRINT(RT_DEBUG_ERROR, (" P2P - 2 P2PSTATUS_NO_CHANNEL .\n")); return; } } /* Check SSID is correct. */ if ((SsidLen > 0) && (!RTMPEqualMemory(pAd->P2pTable.PerstEntry[index].Profile.SSID.Ssid, Ssid, SsidLen))) { DBGPRINT(RT_DEBUG_ERROR, ("Ssid1[%d] = %s. \n", SsidLen, Ssid)); DBGPRINT(RT_DEBUG_ERROR, ("Ssid2[%d] = %s. \n", pAd->P2pTable.PerstEntry[index].Profile.SSID.SsidLength, pAd->P2pTable.PerstEntry[index].Profile.SSID.Ssid)); *pRspStatus = P2PSTATUS_INVALID_PARM; return; } } /* ======================================================================== Routine Description: Check REinvoke's invitation Request frame . Arguments: - NIC Adapter pointer Return Value: FALSE - None of channel in ChannelList Match any channel in pAd->ChannelList[] array IRQL = DISPATCH_LEVEL Note: ======================================================================== */ VOID P2pCheckInviteReqFromExisting( IN PRTMP_ADAPTER pAd, IN PUCHAR ChannelList, IN PUCHAR BssidAddr, IN UCHAR OpChannel, IN PUCHAR Ssid, IN UCHAR SsidLen, IN UCHAR *pRspStatus) { UCHAR i; *pRspStatus = P2PSTATUS_SUCCESS; /* Check if have Common Channels. */ if (FALSE == P2pCheckChannelList(pAd, ChannelList)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - no common channel. Channel list is : %d...\n", ChannelList[0])); *pRspStatus = P2PSTATUS_NO_CHANNEL; return; } /* invite Req from Owner include OpChannel. from Client, doesn't include. */ if (OpChannel != 0) { /* Check Oopchannel is correct. */ for (i = 0;i < pAd->ChannelListNum;i++) { if (pAd->ChannelList[i].Channel == OpChannel) { break; } } if ( i == pAd->ChannelListNum) { *pRspStatus = P2PSTATUS_NO_CHANNEL; DBGPRINT(RT_DEBUG_ERROR, (" P2P - No channel = %d...\n", OpChannel)); return; } } /* Check SSID is correct. */ if ((SsidLen == 0)) { DBGPRINT(RT_DEBUG_ERROR, (" P2P - Ssidlen Zero = %d...\n", SsidLen)); *pRspStatus = P2PSTATUS_INVALID_PARM; return; } } UCHAR ChannelToClass( IN UCHAR Channel, IN UCHAR Country) { UCHAR ReturnClass = 1; if (Country == 1/*COUNTRY_USA*/) { /* 0. Decide current regulatory class. P802.11REVmb_D3.0.pdf. TableJ-4 */ if (Channel <= 11) { ReturnClass = 81; } else if (Channel <= 48) { ReturnClass = 115; /* Atheros suggest to use 1 2010-May */ } else if (Channel <= 64) { ReturnClass = 118; } else if (Channel <= 140) { ReturnClass = 121; } else if ((Channel == 165) || (Channel == 169)) { ReturnClass = 125; } else { /* 3 when channels are 149.153. 157. 161 */ ReturnClass = 124; } } return ReturnClass; }