uint32 reg_proto_get_msg_type(uint32 *msgType, BufferObj *msg) { CTlvVersion version; CTlvMsgType bufMsgType; int err = 0; err |= tlv_dserialize(&version, WPS_ID_VERSION, msg, 0, 0); memset(&bufMsgType, 0, sizeof(CTlvMsgType)); err |= tlv_dserialize(&bufMsgType, WPS_ID_MSG_TYPE, msg, 0, 0); *msgType = bufMsgType.m_data; buffobj_Rewind(msg); if (err) return WPS_ERR_INVALID_PARAMETERS; else return WPS_SUCCESS; }
/* * NOTE: The caller MUST call tlv_delete(ssrmsg->authorizedMacs, 1) to free memory which * allocated in this fcuntion. */ int wps_get_upnpDevSSR(WPSAPI_T *g_mc, void *p_cbData, uint32 length, CTlvSsrIE *ssrmsg) { int ret = WPS_CONT; uint8 data8; DevInfo *info = g_mc->dev_info; TUTRACE((TUTRACE_INFO, "wps_get_upnpDevSSR\n")); if (info->sc_mode == SCMODE_AP_REGISTRAR) { BufferObj *vendorExt_bufObj = NULL; BufferObj *wlidcardMac_bufObj = NULL; /* De-serialize the data to get the TLVs */ BufferObj *bufObj = buffobj_new(); if (!bufObj) return WPS_ERR_SYSTEM; buffobj_dserial(bufObj, (uint8 *)p_cbData, length); memset(ssrmsg, 0, sizeof(CTlvSsrIE)); /* Version */ tlv_dserialize(&ssrmsg->version, WPS_ID_VERSION, bufObj, 0, 0); /* Selected Registrar */ tlv_dserialize(&ssrmsg->selReg, WPS_ID_SEL_REGISTRAR, bufObj, 0, 0); /* Device Password ID */ tlv_dserialize(&ssrmsg->devPwdId, WPS_ID_DEVICE_PWD_ID, bufObj, 0, 0); /* Selected Registrar Config Methods */ tlv_dserialize(&ssrmsg->selRegCfgMethods, WPS_ID_SEL_REG_CFG_METHODS, bufObj, 0, 0); /* WSC 2.0 */ data8 = info->version2; if (data8 >= WPS_VERSION2) { /* Check subelement in WFA Vendor Extension */ if (tlv_find_vendorExtParse(&ssrmsg->vendorExt, bufObj, (uint8 *)WFA_VENDOR_EXT_ID) != 0) goto add_wildcard_mac; /* Deserialize subelement */ vendorExt_bufObj = buffobj_new(); if (!vendorExt_bufObj) { buffobj_del(bufObj); return -1; } buffobj_dserial(vendorExt_bufObj, ssrmsg->vendorExt.vendorData, ssrmsg->vendorExt.dataLength); /* Get Version2 and AuthorizedMACs subelement */ if (subtlv_dserialize(&ssrmsg->version2, WPS_WFA_SUBID_VERSION2, vendorExt_bufObj, 0, 0) == 0) { /* AuthorizedMACs, <= 30B */ subtlv_dserialize(&ssrmsg->authorizedMacs, WPS_WFA_SUBID_AUTHORIZED_MACS, vendorExt_bufObj, 0, 0); } /* Add wildcard MAC when authorized mac not specified */ if (ssrmsg->authorizedMacs.subtlvbase.m_len == 0) { add_wildcard_mac: /* * If the External Registrar is WSC version 1.0 it * will not have included an AuthorizedMACs subelement. * In this case the AP shall add the wildcard MAC Address * (FF:FF:FF:FF:FF:FF) in an AuthorizedMACs subelement in * Beacon and Probe Response frames */ wlidcardMac_bufObj = buffobj_new(); if (!wlidcardMac_bufObj) { buffobj_del(vendorExt_bufObj); buffobj_del(bufObj); return -1; } /* Serialize the wildcard_authorizedMacs to wlidcardMac_Obj */ subtlv_serialize(WPS_WFA_SUBID_AUTHORIZED_MACS, wlidcardMac_bufObj, (char *)wildcard_authorizedMacs, SIZE_MAC_ADDR); buffobj_Rewind(wlidcardMac_bufObj); /* * De-serialize the wlidcardMac_Obj data to get the TLVs * Do allocation, because wlidcardMac_bufObj will be freed here but * ssrmsg->authorizedMacs return and used by caller. */ subtlv_dserialize(&ssrmsg->authorizedMacs, WPS_WFA_SUBID_AUTHORIZED_MACS, wlidcardMac_bufObj, 0, 1); /* Need to free it */ } /* Get UUID-R after WFA-Vendor Extension (wpa_supplicant use this way) */ tlv_find_dserialize(&ssrmsg->uuid_R, WPS_ID_UUID_R, bufObj, 0, 0); } /* Other... */ if (bufObj) buffobj_del(bufObj); if (wlidcardMac_bufObj) buffobj_del(wlidcardMac_bufObj); if (vendorExt_bufObj) buffobj_del(vendorExt_bufObj); ret = WPS_SUCCESS; } return ret; }
uint32 nfc_utils_parse_pw(WpsOobDevPw *devpw, uint8 wsp_version2, uint8 *buf, uint buflen) { uint32 ret = WPS_SUCCESS; char *cp_data; uint16 data16; BufferObj *bufObj = NULL; BufferObj *vendorExt_bufObj = NULL; CTlvVersion version; CSubTlvVersion2 version2; CTlvVendorExt vendorExt; CTlvOobDevPwd *p_oobDevPwd = NULL; /* Sanity check */ if (devpw == NULL || buf == NULL || buflen == 0) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Invalid parameters!\n")); ret = WPS_ERR_INVALID_PARAMETERS; goto error; } if (buflen < 7) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: buf length too short!\n")); ret = WPS_ERR_INVALID_PARAMETERS; goto error; } if (buf[5] != 0x10 || buf[6] != 0x2C) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Not a Password Token!\n")); ret = WPS_ERR_INVALID_PARAMETERS; goto error; } /* Dserial raw data to bufObj */ bufObj = buffobj_new(); if (bufObj == NULL) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Malloc fail!\n")); ret = WPS_ERR_OUTOFMEMORY; goto error; } WPS_HexDumpAscii("NFC_UTILS_PARSE_PW", -1, buf, buflen); buffobj_dserial(bufObj, buf, buflen); /* Version 1 */ if (tlv_dserialize(&version, WPS_ID_VERSION, bufObj, 0, 0) != 0) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: TLV missing\n")); ret = RPROT_ERR_REQD_TLV_MISSING; goto error; } /* Must be 0x10, See WSC2.0 Clause 7.9 Version Negotiation */ if (version.m_data != WPS_VERSION) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Message version should always set to 1\n")); ret = RPROT_ERR_INCOMPATIBLE; goto error; } /* OOB Device Password */ p_oobDevPwd = (CTlvOobDevPwd *)malloc(sizeof(CTlvOobDevPwd)); if (!p_oobDevPwd) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Malloc fail!\n")); ret = WPS_ERR_OUTOFMEMORY; goto error; } memset(p_oobDevPwd, 0, sizeof(CTlvOobDevPwd)); if (tlv_oobDevPwdParse(p_oobDevPwd, bufObj) != 0) { ret = WPS_ERR_SYSTEM; goto error; } /* Save CTlvOobDevPwd to WpsOobDevPw */ memcpy(devpw->pub_key_hash, p_oobDevPwd->publicKeyHash, SIZE_160_BITS); devpw->devPwdId = p_oobDevPwd->pwdId; memcpy(devpw->pin, p_oobDevPwd->ip_devPwd, p_oobDevPwd->devPwdLength); devpw->pin_len = p_oobDevPwd->devPwdLength; /* Version2, here you don't is peer WSC 2.0 or not */ if (wsp_version2 >= WPS_VERSION2) { if (tlv_find_vendorExtParse(&vendorExt, bufObj, (uint8 *)WFA_VENDOR_EXT_ID) != 0) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Cannot find vendor extension\n")); /* ret = RPROT_ERR_INCOMPATIBLE; */ goto error; } /* Deserialize subelement */ if ((vendorExt_bufObj = buffobj_new()) == NULL) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Fail to allocate " "vendor extension buffer, Out of memory\n")); ret = WPS_ERR_OUTOFMEMORY; goto error; } buffobj_dserial(vendorExt_bufObj, vendorExt.vendorData, vendorExt.dataLength); /* Get Version2 subelement */ if ((buffobj_NextSubId(vendorExt_bufObj) != WPS_WFA_SUBID_VERSION2) || (subtlv_dserialize(&version2, WPS_WFA_SUBID_VERSION2, vendorExt_bufObj, 0, 0) != 0)) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Cannot get Version2\n")); /* ret = RPROT_ERR_INCOMPATIBLE; */ goto error; } if (version2.m_data < WPS_VERSION2) { TUTRACE((TUTRACE_NFC, "Pase WPS PW: Invalid Version2 number\n")); ret = RPROT_ERR_INCOMPATIBLE; goto error; } } error: /* free local alloc pointers */ if (vendorExt_bufObj) buffobj_del(vendorExt_bufObj); if (p_oobDevPwd) free(p_oobDevPwd); if (bufObj) buffobj_del(bufObj); return ret; }
uint32 nfc_utils_parse_cfg(WpsEnrCred *cred, uint8 wsp_version2, uint8 *buf, uint buflen) { uint32 ret = WPS_SUCCESS; char *cp_data; uint16 data16; BufferObj *bufObj = NULL; BufferObj *vendorExt_bufObj = NULL; CTlvVersion version; CSubTlvVersion2 version2; CTlvVendorExt vendorExt; CTlvCredential *p_tlvCred = NULL; /* Sanity check */ if (cred == NULL || buf == NULL || buflen == 0) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Invalid parameters!\n")); ret = WPS_ERR_INVALID_PARAMETERS; goto error; } if (buflen < 7) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: buf length too short!\n")); ret = WPS_ERR_INVALID_PARAMETERS; goto error; } if (buf[5] != 0x10 || buf[6] != 0x0E) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Not a Configuration Token!\n")); ret = WPS_ERR_INVALID_PARAMETERS; goto error; } /* Dserial raw data to bufObj */ bufObj = buffobj_new(); if (bufObj == NULL) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Malloc fail!\n")); ret = WPS_ERR_OUTOFMEMORY; goto error; } WPS_HexDumpAscii("NFC_UTILS_PARSE_CFG", -1, buf, buflen); buffobj_dserial(bufObj, buf, buflen); /* Version 1 */ if (tlv_dserialize(&version, WPS_ID_VERSION, bufObj, 0, 0) != 0) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: TLV missing\n")); ret = RPROT_ERR_REQD_TLV_MISSING; goto error; } /* Must be 0x10, See WSC2.0 Clause 7.9 Version Negotiation */ if (version.m_data != WPS_VERSION) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Message version should always set to 1\n")); ret = RPROT_ERR_INCOMPATIBLE; goto error; } /* Configuration */ /* Parse Credential */ p_tlvCred = (CTlvCredential *)malloc(sizeof(CTlvCredential)); if (!p_tlvCred) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Malloc fail!\n")); ret = WPS_ERR_OUTOFMEMORY; goto error; } memset(p_tlvCred, 0, sizeof(CTlvCredential)); tlv_credentialParse(p_tlvCred, bufObj, true); /* Save CTlvCredential to WpsEnrCred */ /* Fill in SSID */ cp_data = (char *)(p_tlvCred->ssid.m_data); data16 = cred->ssidLen = p_tlvCred->ssid.tlvbase.m_len; strncpy(cred->ssid, cp_data, data16); cred->ssid[data16] = '\0'; /* Fill in keyMgmt */ if (p_tlvCred->authType.m_data == WPS_AUTHTYPE_SHARED) { strncpy(cred->keyMgmt, "SHARED", 6); cred->keyMgmt[6] = '\0'; } else if (p_tlvCred->authType.m_data == WPS_AUTHTYPE_WPAPSK) { strncpy(cred->keyMgmt, "WPA-PSK", 7); cred->keyMgmt[7] = '\0'; } else if (p_tlvCred->authType.m_data == WPS_AUTHTYPE_WPA2PSK) { strncpy(cred->keyMgmt, "WPA2-PSK", 8); cred->keyMgmt[8] = '\0'; } else if (p_tlvCred->authType.m_data == (WPS_AUTHTYPE_WPAPSK | WPS_AUTHTYPE_WPA2PSK)) { strncpy(cred->keyMgmt, "WPA-PSK WPA2-PSK", 16); cred->keyMgmt[16] = '\0'; } else { strncpy(cred->keyMgmt, "OPEN", 4); cred->keyMgmt[4] = '\0'; } /* get the real cypher */ cred->encrType = p_tlvCred->encrType.m_data; /* Fill in WEP index, no matter it's WEP type or not */ cred->wepIndex = p_tlvCred->WEPKeyIndex.m_data; /* Fill in PSK */ data16 = p_tlvCred->nwKey.tlvbase.m_len; memset(cred->nwKey, 0, SIZE_64_BYTES); memcpy(cred->nwKey, p_tlvCred->nwKey.m_data, data16); /* we have to set key length here */ cred->nwKeyLen = data16; /* WSC 2.0, nwKeyShareable */ cred->nwKeyShareable = p_tlvCred->nwKeyShareable.m_data; /* Version2, here you don't is peer WSC 2.0 or not */ if (wsp_version2 >= WPS_VERSION2) { if (tlv_find_vendorExtParse(&vendorExt, bufObj, (uint8 *)WFA_VENDOR_EXT_ID) != 0) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Cannot find vendor extension\n")); /* ret = RPROT_ERR_INCOMPATIBLE; */ goto error; } /* Deserialize subelement */ if ((vendorExt_bufObj = buffobj_new()) == NULL) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Fail to allocate " "vendor extension buffer, Out of memory\n")); ret = WPS_ERR_OUTOFMEMORY; goto error; } buffobj_dserial(vendorExt_bufObj, vendorExt.vendorData, vendorExt.dataLength); /* Get Version2 subelement */ if ((buffobj_NextSubId(vendorExt_bufObj) != WPS_WFA_SUBID_VERSION2) || (subtlv_dserialize(&version2, WPS_WFA_SUBID_VERSION2, vendorExt_bufObj, 0, 0) != 0)) { TUTRACE((TUTRACE_NFC, "Pase WPS CFG: Cannot get Version2\n")); /* ret = RPROT_ERR_INCOMPATIBLE; */ goto error; } if (version2.m_data < WPS_VERSION2) { TUTRACE((TUTRACE_NFC, "Pase WPS NDEF: Invalid Version2 number\n")); ret = RPROT_ERR_INCOMPATIBLE; goto error; } } error: /* free local alloc pointers */ if (vendorExt_bufObj) buffobj_del(vendorExt_bufObj); if (p_tlvCred) tlv_credentialDelete(p_tlvCred, 0); if (bufObj) buffobj_del(bufObj); return ret; }