wps_enrollee.c

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

	*(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_M3; // M3
	MessageLength += 2+2+E_MessageType_Length;
	p += 2+2+E_MessageType_Length;

	WPS_set_word(p, WPS_h2n16(E_RegistrarNonce_ID));
	WPS_set_word(p+2, WPS_h2n16(E_RegistrarNonce_Length));
	WPS_MEM_CPY(p+4, enrollee->RegistrarNonce, E_RegistrarNonce_Length);
	MessageLength += 2+2+E_RegistrarNonce_Length;
	p += 2+2+E_RegistrarNonce_Length;

	WPS_set_word(p, WPS_h2n16(E_EHash1_ID));
	WPS_set_word(p+2, WPS_h2n16(E_EHash1_Length));
	WPS_MEM_CPY(p+4, enrollee->EHash1, E_EHash1_Length);
	MessageLength += 2+2+E_EHash1_Length;
	p += 2+2+E_EHash1_Length;

	WPS_set_word(p, WPS_h2n16(E_EHash2_ID));
	WPS_set_word(p+2, WPS_h2n16(E_EHash2_Length));
	WPS_MEM_CPY(p+4, enrollee->EHash2, E_EHash2_Length);
	MessageLength += 2+2+E_EHash2_Length;
	p += 2+2+E_EHash2_Length;

	//
	// Some optional data elements
	//

	WPS_u8 Output[32];
	WPS_u8 *Input = (WPS_u8 *)WPS_MALLOC(enrollee->LastMessageLength+MessageLength);
	WPS_MEM_CPY(Input, enrollee->LastMessage, enrollee->LastMessageLength);
	WPS_MEM_CPY((Input+enrollee->LastMessageLength), MessageData, MessageLength);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, Input, (enrollee->LastMessageLength+MessageLength), Output, 32);
	WPS_MFREE(Input);

	WPS_set_word(p, WPS_h2n16(E_Authenticator_ID));
	WPS_set_word(p+2, WPS_h2n16(E_Authenticator_Length));
	WPS_MEM_CPY(p+4, Output, E_Authenticator_Length); // Only the first 8 bytes of Output
	MessageLength += 2+2+E_Authenticator_Length;
	p += 2+2+E_Authenticator_Length;

	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("SendM3: 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_F;
	enrollee->e_step = E_LOCK_STEP_M3_SENT;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("SendM3: Enrollee state change to E_STATE_F\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_m5(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));
	*(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_M5; // M5
	MessageLength += 2+2+E_MessageType_Length;
	p += 2+2+E_MessageType_Length;

	WPS_set_word(p, WPS_h2n16(E_RegistrarNonce_ID));
	WPS_set_word(p+2, WPS_h2n16(E_RegistrarNonce_Length));
	WPS_MEM_CPY(p+4, enrollee->RegistrarNonce, E_RegistrarNonce_Length);
	MessageLength += 2+2+E_RegistrarNonce_Length;
	p += 2+2+E_RegistrarNonce_Length;

	//
	// Encrypted Settings: E-S1
	//

	WPS_u8 DataToEncrypt[2+2+E_ESNonce1_Length];
	WPS_set_word(DataToEncrypt, WPS_h2n16(E_ESNonce1_ID));
	WPS_set_word((DataToEncrypt+2), WPS_h2n16(E_ESNonce1_Length));
	WPS_MEM_CPY(DataToEncrypt+4, enrollee->ESNonce1, E_ESNonce1_Length);
	wps_enrol_build_enc_set(enrollee, DataToEncrypt, 2+2+E_ESNonce1_Length, p, SEND_BUFFER_SIZE-EAP_HEAD_LENGTH-MessageLength);
	//MessageLength += 2+2+16+2+2+E_ESNonce1_Length+2+2+E_KeyWrapAuthenticator_Length+16; // 16 bytes stuff
	//p += 2+2+16+2+2+E_ESNonce1_Length+2+2+E_KeyWrapAuthenticator_Length+16; // 16 bytes stuff
	MessageLength += 2+2+WPS_n2h16(WPS_get_word(p+2));
	p += 2+2+WPS_n2h16(WPS_get_word(p+2));

	//
	// Some optional data elements
	//

	WPS_u8 Output[32];
	WPS_u8 *Input = (WPS_u8 *)WPS_MALLOC(enrollee->LastMessageLength+MessageLength);
	WPS_MEM_CPY(Input, enrollee->LastMessage, enrollee->LastMessageLength);
	WPS_MEM_CPY((Input+enrollee->LastMessageLength), MessageData, MessageLength);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, Input, (enrollee->LastMessageLength+MessageLength), Output, 32);
	WPS_MFREE(Input);

	WPS_set_word(p, WPS_h2n16(E_Authenticator_ID));
	WPS_set_word(p+2, WPS_h2n16(E_Authenticator_Length));
	WPS_MEM_CPY(p+4, Output, E_Authenticator_Length); // Only the first 8 bytes of Output
	MessageLength += 2+2+E_Authenticator_Length;
	p += 2+2+E_Authenticator_Length;

	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("SendM5: 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_F;
	enrollee->e_step = E_LOCK_STEP_M5_SENT;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("SendM5: Enrollee state change to E_STATE_F\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_m7(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));
	*(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_M7; // M7
	MessageLength += 2+2+E_MessageType_Length;
	p += 2+2+E_MessageType_Length;

	WPS_set_word(p, WPS_h2n16(E_RegistrarNonce_ID));
	WPS_set_word(p+2, WPS_h2n16(E_RegistrarNonce_Length));
	WPS_MEM_CPY(p+4, enrollee->RegistrarNonce, E_RegistrarNonce_Length);
	MessageLength += 2+2+E_RegistrarNonce_Length;
	p += 2+2+E_RegistrarNonce_Length;

	//
	// Encrypted Settings: E-S2
	//
	WPS_u8 DataToEncrypt[2+2+E_ESNonce2_Length];
	WPS_set_word(DataToEncrypt, WPS_h2n16(E_ESNonce2_ID));
	WPS_set_word((DataToEncrypt+2), WPS_h2n16(E_ESNonce2_Length));
	WPS_MEM_CPY(DataToEncrypt+4, enrollee->ESNonce2, E_ESNonce2_Length);
	wps_enrol_build_enc_set(enrollee, DataToEncrypt, 2+2+E_ESNonce2_Length, p, SEND_BUFFER_SIZE-EAP_HEAD_LENGTH-MessageLength);
	//MessageLength += 2+2+16+2+2+E_ESNonce2_Length+2+2+E_KeyWrapAuthenticator_Length+16; // 16 bytes stuff
	//p += 2+2+16+2+2+E_ESNonce2_Length+2+2+E_KeyWrapAuthenticator_Length+16; // 16 bytes stuff
	MessageLength += 2+2+WPS_n2h16(WPS_get_word(p+2));
	p += 2+2+WPS_n2h16(WPS_get_word(p+2));

	//
	// Some optional data elements
	//

	WPS_u8 Output[32];
	WPS_u8 *Input = (WPS_u8 *)WPS_MALLOC(enrollee->LastMessageLength+MessageLength);
	WPS_MEM_CPY(Input, enrollee->LastMessage, enrollee->LastMessageLength);
	WPS_MEM_CPY((Input+enrollee->LastMessageLength), MessageData, MessageLength);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, Input, (enrollee->LastMessageLength+MessageLength), Output, 32);
	WPS_MFREE(Input);

	WPS_set_word(p, WPS_h2n16(E_Authenticator_ID));
	WPS_set_word(p+2, WPS_h2n16(E_Authenticator_Length));
	WPS_MEM_CPY(p+4, Output, E_Authenticator_Length); // Only the first 8 bytes of Output
	MessageLength += 2+2+E_Authenticator_Length;
	p += 2+2+E_Authenticator_Length;

	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("SendM7: 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_F;
	enrollee->e_step = E_LOCK_STEP_M7_SENT;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("SendM7: Enrollee state change to E_STATE_F\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_ack(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));
	*(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_WSC_ACK; // WSC_ACK
	MessageLength += 2+2+E_MessageType_Length;
	p += 2+2+E_MessageType_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_RegistrarNonce_ID));
	WPS_set_word(p+2, WPS_h2n16(E_RegistrarNonce_Length));
	WPS_MEM_CPY(p+4, enrollee->RegistrarNonce, E_RegistrarNonce_Length);
	MessageLength += 2+2+E_RegistrarNonce_Length;
	p += 2+2+E_RegistrarNonce_Length;

	//
	// Some optional data elements
	//


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

	WPS_s32 ret = Wlan_SendEapPacket(SendBuff, EAP_HEAD_LENGTH + MessageLength);
	if (ret < 0)
	{
		WPS_PRINTF("SendACK: 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
	//
	if (enrollee->e_state == E_STATE_E)
	{
		enrollee->e_state = E_STATE_D;
#if ENROLLEE_DEBUG		
		WPS_PRINTF("SendACK: Enrollee state change to E_STATE_D\n");
#endif		
	}

	//
	// Update LastMessage point
	// This is not needed!   20070910
	//
	//if (wps_enrol_upt_last_msg_for_snd(enrollee, SendBuff, MessageLength) == -1)
	//{
	//	return -1;
	//}

	return 0;
}

//
// Before calling this function, enrollee->ConfigurationError_E should be set.
//
WPS_s32 wps_enrol_snd_nack(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));
	*(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_WSC_NACK; // WSC_NACK
	MessageLength += 2+2+E_MessageType_Length;
	p += 2+2+E_MessageType_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_RegistrarNonce_ID));
	WPS_set_word(p+2, WPS_h2n16(E_RegistrarNonce_Length));
	WPS_MEM_CPY(p+4, enrollee->RegistrarNonce, E_RegistrarNonce_Length);
	MessageLength += 2+2+E_RegistrarNonce_Length;
	p += 2+2+E_RegistrarNonce_Length;

	//
	// Configuration Error
	//
	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;

	//
	// Some optional data elements
	//


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

	WPS_s32 ret = Wlan_SendEapPacket(SendBuff, EAP_HEAD_LENGTH + MessageLength);
	if (ret < 0)
	{
		WPS_PRINTF("SendNACK: 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
	//
	if (enrollee->e_state == E_STATE_E)
	{
		enrollee->e_state = E_STATE_G;
#if ENROLLEE_DEBUG		
		WPS_PRINTF("SendNACK: Enrollee state change to E_STATE_G\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_done(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;