wps_enrollee.c

WiFi Protected Setup (WPS) 又叫Simple config。 是无线局域网领域推出的新协议

{
	WPS_s32 ret;
	WPS_s16 AttributeDataLength = WPS_n2h16(WPS_get_word((WPS_u8 *)Input+2));
	const WPS_u8 *IV = Input+4;

	WPS_u8 IV_temp[16];
	WPS_MEM_CPY(IV_temp, IV, 16);

	WPS_u8 *temp = (WPS_u8 *)WPS_MALLOC(AttributeDataLength-16);
	WPS_AES_CBC_Decrypt(enrollee->KeyWrapKey, 16, IV_temp, 16, Input+4+16, AttributeDataLength-16, temp, AttributeDataLength-16);

#if ENROLLEE_DEBUG	
	WPS_PRINTF("-----------------Data||KWA (%d bytes)---------------\n", AttributeDataLength-16);
	PrintHex(temp, AttributeDataLength-16);
	WPS_PRINTF("--------------------------------------------------\n");
#endif	

	WPS_u8 *p = temp;
	WPS_u8 *HMAC_Input = temp;
	WPS_s16 HMAC_InputLen = 0;

	// Calculate the HMAC_InputLen, all elements before "KeyWrapAuthenticator".
	while (WPS_n2h16(WPS_get_word(p)) != E_KeyWrapAuthenticator_ID)
	{
		HMAC_InputLen += 2+2+WPS_n2h16(WPS_get_word(p+2));
		p = temp+HMAC_InputLen;
	}

#if ENROLLEE_DEBUG	
	WPS_PRINTF("HMAC_InputLen = %d\n", HMAC_InputLen);
#endif	

	WPS_u8 HMAC_Output[32];
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, HMAC_Input, HMAC_InputLen, HMAC_Output, 32);
	if (WPS_MEM_CMP(HMAC_Output, temp+HMAC_InputLen+2+2, E_KeyWrapAuthenticator_Length) == 0)
	{
		WPS_MEM_CPY(Output, temp, (HMAC_InputLen > OutputBuffLen ? OutputBuffLen : HMAC_InputLen));
		ret = 0;
	}
	else
	{
		ret = -1;
	}
	WPS_MFREE(temp);
	return ret;
}

//
// Function to compute Diffie-Hellman Share Key,
// and then calculate KDK and AuthKey, KeyWrapKey, EMSK.
// Return values:
//     0: Successful.
//     -1: Failed.
//
WPS_s32 wps_enrol_key_derivation(struct enrollee *enrollee)
{
#if ENROLLEE_DEBUG	
	WPS_PRINTF("\nEnrollee Key Infomation:\n");
#endif
	//
	// Compute KDK...
	// KDK = HMAC-SHA256,DHKey (N1 || EnrolleeMAC || N2)
	//
	WPS_s32 DHKeyLen = WPS_dh_sharedkey_size(enrollee->DH_e);
	WPS_u8 *DHKey = (WPS_u8 *)WPS_MALLOC(DHKeyLen);
	if (WPS_dh_compute_sharedkey(
		enrollee->DH_e, 
		enrollee->PK_R, 
		E_PublicKey_Length,
		DHKey,
		DHKeyLen) == -1)
	{
		WPS_PRINTF("wps_enrol_key_derivation: Compute DH shared key failed!\n");
		WPS_MFREE(DHKey);
		return -1;
	}

#if 0
	WPS_PRINTF("\n---------PK_E--------------------------\n");
	PrintHex(enrollee->PK_E, E_PublicKey_Length);

	WPS_PRINTF("\n---------PK_R--------------------------\n");
	PrintHex(enrollee->PK_R, E_PublicKey_Length);

	WPS_PRINTF("\n---------Diffie-Hellman Share Key------\n");
	PrintHex(DHKey, DHKeyLen);
#endif

	WPS_u8 DHKey2[32];
	WPS_Sha256(DHKey, DHKeyLen, DHKey2, 32);

	WPS_s32 InputLen = E_EnrolleeNonce_Length+E_MACAddress_Length+E_RegistrarNonce_Length;
	WPS_u8 *Input = (WPS_u8 *)WPS_MALLOC(InputLen);// 16+6+16=38
	WPS_MEM_CPY(Input, enrollee->EnrolleeNonce, E_EnrolleeNonce_Length);
	WPS_MEM_CPY(Input+E_EnrolleeNonce_Length, enrollee->EnrolleeMAC, E_MACAddress_Length);
	WPS_MEM_CPY(Input+E_EnrolleeNonce_Length+E_MACAddress_Length, enrollee->RegistrarNonce, E_EnrolleeNonce_Length);

	WPS_HMAC_Sha256(DHKey2, 32, Input, InputLen, enrollee->KDK, 32);

	WPS_MFREE(Input);
	WPS_MFREE(DHKey);

	/* Given KDK and key derivation function, 
	 * the Registration Protocol session keys are derived as follows:
	 * AuthKey || KeyWrapKey || EMSK = kdf(KDK, "Wi-Fi Easy and Secure Key Derivation", 640) 
	 */
	WPS_u8 CombiKey[640/8];

	kdf(enrollee->KDK, 32, "Wi-Fi Easy and Secure Key Derivation", CombiKey, 640);
	WPS_MEM_CPY(enrollee->AuthKey, CombiKey, 32);
	WPS_MEM_CPY(enrollee->KeyWrapKey, CombiKey+32, 16);
	WPS_MEM_CPY(enrollee->EMSK, CombiKey+32+16, 32);

#if ENROLLEE_DEBUG
	WPS_PRINTF("\n---------AuthKey-----------------------\n");
	PrintHex(enrollee->AuthKey, 32);

	WPS_PRINTF("\n---------KeyWrapKey--------------------\n");
	PrintHex(enrollee->KeyWrapKey, 16);

	WPS_PRINTF("\n---------EMSK--------------------------\n");
	PrintHex(enrollee->EMSK, 32);
	WPS_PRINTF("\n");
#endif

	return 0;
}

//
// Return values:
//  0: OK
// -1: Invalid
//
WPS_s32 wps_enrol_check_rh1_using_rsn1(struct enrollee *enrollee)
{
	WPS_u8 TempRHash1[E_RHash1_Length];

	WPS_u8 TempInput1[ E_RSNonce1_Length+16+E_PublicKey_Length+E_PublicKey_Length ];
	WPS_MEM_CPY(TempInput1, enrollee->RSNonce1, E_RSNonce1_Length);
	WPS_MEM_CPY(TempInput1+E_RSNonce1_Length, enrollee->PSK1, 16);
	WPS_MEM_CPY(TempInput1+E_RSNonce1_Length+16, enrollee->PK_E, E_PublicKey_Length);
	WPS_MEM_CPY(TempInput1+E_RSNonce1_Length+16+E_PublicKey_Length, enrollee->PK_R, E_PublicKey_Length);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, TempInput1, E_RSNonce1_Length+16+E_PublicKey_Length+E_PublicKey_Length, TempRHash1, E_RHash1_Length);

	if (WPS_MEM_CMP(enrollee->RHash1, TempRHash1, E_RHash1_Length) == 0)
	{
		return 0;
	}
	else
	{
		return -1;
	}
}

//
// Return values:
//  0: OK
// -1: Invalid
//
WPS_s32 wps_enrol_check_rh2_using_rsn2(struct enrollee *enrollee)
{
	WPS_u8 TempRHash2[E_RHash2_Length];

	WPS_u8 TempInput2[ E_RSNonce2_Length+16+E_PublicKey_Length+E_PublicKey_Length ];
	WPS_MEM_CPY(TempInput2, enrollee->RSNonce2, E_RSNonce2_Length);
	WPS_MEM_CPY(TempInput2+E_RSNonce2_Length, enrollee->PSK2, 16);
	WPS_MEM_CPY(TempInput2+E_RSNonce2_Length+16, enrollee->PK_E, E_PublicKey_Length);
	WPS_MEM_CPY(TempInput2+E_RSNonce2_Length+16+E_PublicKey_Length, enrollee->PK_R, E_PublicKey_Length);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, TempInput2, E_RSNonce2_Length+16+E_PublicKey_Length+E_PublicKey_Length, TempRHash2, E_RHash2_Length);

	if (WPS_MEM_CMP(enrollee->RHash2, TempRHash2, E_RHash2_Length) == 0)
	{
		return 0;
	}
	else
	{
		return -1;
	}
}

WPS_s32 wps_enrol_upt_last_msg_for_snd(struct enrollee *enrollee, WPS_u8 *SendBuff, WPS_s16 MessageLength)
{
	if (enrollee->LastPacket != NULL)
	{
		WPS_MFREE(enrollee->LastPacket);
		enrollee->LastPacket = NULL;
	}
	WPS_s32 LastPacketLen = EAP_HEAD_LENGTH+MessageLength;
	WPS_u8 *temp = (WPS_u8 *)WPS_MALLOC(LastPacketLen);
	if (temp == NULL)
	{
		WPS_PRINTF("wps_enrol_upt_last_msg_for_snd: no enough memory\n");
		return -1;
	}
	WPS_MEM_CPY(temp, SendBuff, LastPacketLen);
	enrollee->LastPacket = temp;
	enrollee->LastPacketLength = LastPacketLen;
	enrollee->LastMessage = enrollee->LastPacket+EAP_HEAD_LENGTH;
	enrollee->LastMessageLength = MessageLength;
	return 0;
}

WPS_s32 wps_enrol_upt_last_msg_for_rcv(struct enrollee *enrollee, struct eap_packet *packet)
{
	if (enrollee->LastPacket != NULL)
	{
		WPS_MFREE(enrollee->LastPacket);
		enrollee->LastPacket = NULL;
	}
	WPS_s32 LastPacketLen = WPS_n2h16(packet->Length);
	WPS_u8 *temp = (WPS_u8 *)WPS_MALLOC(LastPacketLen);
	if (temp == NULL)
	{
		WPS_PRINTF("wps_enrol_upt_last_msg_for_rcv: no enough memory\n");
		return -1;
	}
	WPS_MEM_CPY(temp, (WPS_u8 *)packet, LastPacketLen);
	enrollee->LastPacket = temp;
	enrollee->LastPacketLength = LastPacketLen;
	enrollee->LastMessage = enrollee->LastPacket+EAP_HEAD_LENGTH;
	enrollee->LastMessageLength = EAP_MESSAGE_LENGTH(packet);
	return 0;
}

/////////////////////////////////////////////////////////////////////////////////////

WPS_s32 wps_enrol_snd_m1(struct enrollee *enrollee)
{
	WPS_u8 SendBuff[SEND_BUFFER_SIZE];
	struct eap_packet* packet = (struct eap_packet*)(SendBuff);
	WPS_u8 *MessageData = SendBuff + EAP_HEAD_LENGTH;
	WPS_u8 *p = MessageData;
	WPS_s16 MessageLength = 0;
	WPS_s16 DataLength;

	//
	// To fill the MessageData
	//
	WPS_set_word(p, WPS_h2n16(E_Version_ID));
	WPS_set_word(p+2, WPS_h2n16(E_Version_Length));
	*(p+4) = enrollee->MyVersion;
	MessageLength += 2+2+E_Version_Length;
	p += 2+2+E_Version_Length;

	WPS_set_word(p, WPS_h2n16(E_MessageType_ID));
	WPS_set_word(p+2, WPS_h2n16(E_MessageType_Length));
	*(p+4) = MESSAGE_TYPE_M1; // M1
	MessageLength += 2+2+E_MessageType_Length;
	p += 2+2+E_MessageType_Length;

	WPS_set_word(p, WPS_h2n16(E_UUID_E_ID));
	WPS_set_word(p+2, WPS_h2n16(E_UUID_E_Length));
	WPS_MEM_CPY(p+4, enrollee->UUID_E, E_UUID_E_Length);
	MessageLength += 2+2+E_UUID_E_Length;
	p += 2+2+E_UUID_E_Length;

	WPS_set_word(p, WPS_h2n16(E_MACAddress_ID));
	WPS_set_word(p+2, WPS_h2n16(E_MACAddress_Length));
	WPS_MEM_CPY(p+4, enrollee->EnrolleeMAC, E_MACAddress_Length);
	MessageLength += 2+2+E_MACAddress_Length;
	p += 2+2+E_MACAddress_Length;

	WPS_set_word(p, WPS_h2n16(E_EnrolleeNonce_ID));
	WPS_set_word(p+2, WPS_h2n16(E_EnrolleeNonce_Length));
	WPS_MEM_CPY(p+4, enrollee->EnrolleeNonce, E_EnrolleeNonce_Length);
	MessageLength += 2+2+E_EnrolleeNonce_Length;
	p += 2+2+E_EnrolleeNonce_Length;

	WPS_set_word(p, WPS_h2n16(E_PublicKey_ID));
	WPS_set_word(p+2, WPS_h2n16(E_PublicKey_Length));
	WPS_MEM_CPY(p+4, enrollee->PK_E, E_PublicKey_Length);
	MessageLength += 2+2+E_PublicKey_Length;
	p += 2+2+E_PublicKey_Length;

	WPS_set_word(p, WPS_h2n16(E_AuthenticationTypeFlags_ID));
	WPS_set_word(p+2, WPS_h2n16(E_AuthenticationTypeFlags_Length));
	WPS_set_word((p+4), WPS_h2n16(enrollee->AuthenticationTypeFlags_E));
	MessageLength += 2+2+E_AuthenticationTypeFlags_Length;
	p += 2+2+E_AuthenticationTypeFlags_Length;

	WPS_set_word(p, WPS_h2n16(E_EncryptionTypeFlags_ID));
	WPS_set_word(p+2, WPS_h2n16(E_EncryptionTypeFlags_Length));
	WPS_set_word((p+4), WPS_h2n16(enrollee->EncryptionTypeFlags_E));
	MessageLength += 2+2+E_EncryptionTypeFlags_Length;
	p += 2+2+E_EncryptionTypeFlags_Length;

	WPS_set_word(p, WPS_h2n16(E_ConnectionTypeFlags_ID));
	WPS_set_word(p+2, WPS_h2n16(E_ConnectionTypeFlags_Length));
	*(p+4) = enrollee->ConnectionTypeFlags_E;
	MessageLength += 2+2+E_ConnectionTypeFlags_Length;
	p += 2+2+E_ConnectionTypeFlags_Length;

	WPS_set_word(p, WPS_h2n16(E_ConfigMethods_ID));
	WPS_set_word(p+2, WPS_h2n16(E_ConfigMethods_Length));
	WPS_set_word((p+4), WPS_h2n16(enrollee->ConfigMethods_E));
	MessageLength += 2+2+E_ConfigMethods_Length;
	p += 2+2+E_ConfigMethods_Length;

	WPS_set_word(p, WPS_h2n16(E_SimpleConfigState_ID));
	WPS_set_word(p+2, WPS_h2n16(E_SimpleConfigState_Length));
	*(p+4) = enrollee->SimpleConfigState_E;
	MessageLength += 2+2+E_SimpleConfigState_Length;
	p += 2+2+E_SimpleConfigState_Length;

	DataLength = WPS_STR_LEN((WPS_char *)(enrollee->Manufacturer_E))+1;
	WPS_set_word(p, WPS_h2n16(E_Manufacturer_ID));
	WPS_set_word(p+2, WPS_h2n16(DataLength));
	WPS_MEM_CPY(p+4, enrollee->Manufacturer_E, DataLength);
	MessageLength += 2+2+DataLength;
	p += 2+2+DataLength;

	DataLength = WPS_STR_LEN((WPS_char *)(enrollee->ModelName_E))+1;
	WPS_set_word(p, WPS_h2n16(E_ModelName_ID));
	WPS_set_word(p+2, WPS_h2n16(DataLength));
	WPS_MEM_CPY(p+4, enrollee->ModelName_E, DataLength);
	MessageLength += 2+2+DataLength;
	p += 2+2+DataLength;

	DataLength = WPS_STR_LEN((WPS_char *)(enrollee->ModelNumber_E))+1;
	WPS_set_word(p, WPS_h2n16(E_ModelNumber_ID));
	WPS_set_word(p+2, WPS_h2n16(DataLength));
	WPS_MEM_CPY(p+4, enrollee->ModelNumber_E, DataLength);
	MessageLength += 2+2+DataLength;
	p += 2+2+DataLength;

	DataLength = WPS_STR_LEN((WPS_char *)(enrollee->SerialNumber_E))+1;
	WPS_set_word(p, WPS_h2n16(E_SerialNumber_ID));
	WPS_set_word(p+2, WPS_h2n16(DataLength));
	WPS_MEM_CPY(p+4, enrollee->SerialNumber_E, DataLength);
	MessageLength += 2+2+DataLength;
	p += 2+2+DataLength;

	WPS_set_word(p, WPS_h2n16(E_PrimaryDeviceType_ID));
	WPS_set_word(p+2, WPS_h2n16(E_PrimaryDeviceType_Length));
	WPS_MEM_CPY(p+4, &(enrollee->primary_device_type_E), E_PrimaryDeviceType_Length);
	MessageLength += 2+2+E_PrimaryDeviceType_Length;
	p += 2+2+E_PrimaryDeviceType_Length;

	DataLength = WPS_STR_LEN((WPS_char *)(enrollee->DeviceName_E))+1;
	WPS_set_word(p, WPS_h2n16(E_DeviceName_ID));
	WPS_set_word(p+2, WPS_h2n16(DataLength));
	WPS_MEM_CPY(p+4, enrollee->DeviceName_E, DataLength);
	MessageLength += 2+2+DataLength;
	p += 2+2+DataLength;

	WPS_set_word(p, WPS_h2n16(E_RFBands_ID));
	WPS_set_word(p+2, WPS_h2n16(E_RFBands_Length));
	*(p+4) = enrollee->RFBands_E;
	MessageLength += 2+2+E_RFBands_Length;
	p += 2+2+E_RFBands_Length;

	WPS_set_word(p, WPS_h2n16(E_AssociationState_ID));
	WPS_set_word(p+2, WPS_h2n16(E_AssociationState_Length));
	WPS_set_word((p+4), WPS_h2n16(enrollee->AssociationState_E));
	MessageLength += 2+2+E_AssociationState_Length;
	p += 2+2+E_AssociationState_Length;

	WPS_set_word(p, WPS_h2n16(E_DevicePasswordID_ID));
	WPS_set_word(p+2, WPS_h2n16(E_DevicePasswordID_Length));
	WPS_set_word((p+4), WPS_h2n16(enrollee->DevicePasswordID_E));
	MessageLength += 2+2+E_DevicePasswordID_Length;
	p += 2+2+E_DevicePasswordID_Length;

	WPS_set_word(p, WPS_h2n16(E_ConfigurationError_ID));
	WPS_set_word(p+2, WPS_h2n16(E_ConfigurationError_Length));
	WPS_set_word((p+4), WPS_h2n16(enrollee->ConfigurationError_E));
	MessageLength += 2+2+E_ConfigurationError_Length;
	p += 2+2+E_ConfigurationError_Length;

	WPS_set_word(p, WPS_h2n16(E_OSVersion_ID));
	WPS_set_word(p+2, WPS_h2n16(E_OSVersion_Length));
	WPS_set_dword(p+4, WPS_h2n32(enrollee->OSVersion_E));
	MessageLength += 2+2+E_OSVersion_Length;
	p += 2+2+E_OSVersion_Length;

	//
	// Some optional data elements
	//

	BuildEAPHead(packet, EAP_RESPONSE_CODE, enrollee->eap_id, WSC_MSG, MessageLength);

	WPS_s32 ret = Wlan_SendEapPacket(SendBuff, EAP_HEAD_LENGTH + MessageLength);
	if (ret < 0)
	{
		WPS_PRINTF("SendM1: packet send failed!\n");
		return -1;
	}
	WPS_StartTimer(enrollee->resend_timer, WPS_RETRANSMISSION_TIMEOUT * 1000, wps_resend_packet_timeout, (void *)enrollee);
	WPS_StartTimer(enrollee->permessage_timer, WPS_PER_PACKET_TIMEOUT * 1000, wps_permessage_timeout, (void *)enrollee);

	//
	// Update Enrollee state machine
	//
	enrollee->e_state = E_STATE_D;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("SendM1: Enrollee state change to E_STATE_D\n");
#endif

	//
	// Update LastMessage point
	//
	if (wps_enrol_upt_last_msg_for_snd(enrollee, SendBuff, MessageLength) == -1)
	{
		return -1;
	}

	return 0;
}

WPS_s32 wps_enrol_snd_m3(struct enrollee *enrollee)
{
	WPS_u8 SendBuff[SEND_BUFFER_SIZE];
	struct eap_packet* packet = (struct eap_packet*)(SendBuff);
	WPS_u8 *MessageData = SendBuff + EAP_HEAD_LENGTH;
	WPS_u8 *p = MessageData;
	WPS_s16 MessageLength = 0;

	//
	// To fill the MessageData
	//
	WPS_set_word(p, WPS_h2n16(E_Version_ID));
	WPS_set_word(p+2, WPS_h2n16(E_Version_Length));