wps_enrollee.c

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

	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_DONE; // WSC_Done
	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_Done, MessageLength);

	WPS_s32 ret = Wlan_SendEapPacket(SendBuff, EAP_HEAD_LENGTH + MessageLength);
	if (ret < 0)
	{
		WPS_PRINTF("SendDone: 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->success = 1;
	enrollee->e_state = E_STATE_H;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("SendDone: Enrollee state change to E_STATE_H\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_eap_fail(struct enrollee *enrollee)
{
	WPS_u8 SendBuff[SEND_BUFFER_SIZE];
	struct eap_packet* packet = (struct eap_packet*)(SendBuff);
	/*WPS_u8 *MessageData = SendBuff + EAP_HEAD_LENGTH;*/

	/* Build EAP Head */
	packet->Code = EAP_FAILURE_CODE;
	packet->Identifier = 0;
	packet->Length = WPS_h2n16(4);

	WPS_s32 ret = Wlan_SendEapPacket(SendBuff, 4);
	if (ret < 0)
	{
		WPS_PRINTF("Send_EAP_Fail: packet send failed!\n");
		return -1;
	}

	enrollee->done = 1;
	enrollee->e_state = E_STATE_A;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("Send_EAP_Fail: Enrollee state change to E_STATE_A\n");
#endif

	return 0;
}

WPS_s32 wps_enrol_snd_eap_rsp_ident(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_STR_CPY((WPS_u8*)packet+5, NAI_ENROLLEE);	
	
	/* Build EAP Head */
	packet->Code = EAP_RESPONSE_CODE;
	packet->Identifier = enrollee->eap_id;
	packet->Length = WPS_h2n16(4+1+WPS_STR_LEN(NAI_ENROLLEE));
	*((WPS_u8*)packet+4) = WPS_IDENTIFY_TYPE;

	WPS_s32 ret = Wlan_SendEapPacket(SendBuff, 4+1+WPS_STR_LEN(NAI_ENROLLEE));
	if (ret < 0)
	{
		WPS_PRINTF("Send_EAP_Response_Identity: 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_B;
#if ENROLLEE_DEBUG	
	WPS_PRINTF("Send_EAP_Response_Identity: Enrollee state change to E_STATE_B\n");
#endif

	//
	// Update LastMessage point
	//
	if (enrollee->LastPacket != NULL)
	{
		WPS_MFREE(enrollee->LastPacket);
		enrollee->LastPacket = NULL;
	}
	WPS_s32 LastPacketLen = 4+1+WPS_STR_LEN(NAI_ENROLLEE);
	WPS_u8 *temp = (WPS_u8 *)WPS_MALLOC(LastPacketLen);
	if (temp == NULL)
	{
		WPS_PRINTF("Send_EAP_Response_Identity: no enough memory\n");
		return -1;
	}
	WPS_MEM_CPY(temp, SendBuff, LastPacketLen);
	enrollee->LastPacket = temp;
	enrollee->LastPacketLength = LastPacketLen;
	enrollee->LastMessage = NULL;
	enrollee->LastMessageLength = 0;

	return 0;
}

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

WPS_s32 wps_enrol_rcv_start(struct enrollee *enrollee, struct eap_packet *packet)
{
	if (enrollee->e_state != E_STATE_B || enrollee->e_step != E_LOCK_STEP_NO_LOCKED)
	{
		WPS_PRINTF("Enrollee: State is incorrect to receive WSC_Start\n");
#if ENROLLEE_CHECK_EAP_IDENTIFIER			
		enrollee->ConfigurationError_E = ERROR_STATE_ERROR;
		wps_enrol_snd_nack(enrollee);
#endif		
		return -1;
	}

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

	return wps_enrol_snd_m1(enrollee);
}

WPS_s32 wps_enrol_rcv_m2(struct enrollee *enrollee, struct eap_packet *packet)
{
	if (enrollee->e_state != E_STATE_D || enrollee->e_step != E_LOCK_STEP_NO_LOCKED)
	{
		WPS_PRINTF("Enrollee: State is incorrect to receive M2\n");
#if ENROLLEE_CHECK_EAP_IDENTIFIER			
		enrollee->ConfigurationError_E = ERROR_STATE_ERROR;
		wps_enrol_snd_nack(enrollee);
#endif		
		return -1;
	}

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

	WPS_u8 *p = (WPS_u8 *)packet+EAP_HEAD_LENGTH+10;
	WPS_u16 DataLen = 0;

	//
	// Enrollee Nonce
	//
	if (WPS_get_word(p) != WPS_h2n16(E_EnrolleeNonce_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_EnrolleeNonce_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		if (WPS_MEM_CMP(enrollee->EnrolleeNonce, p+4, E_EnrolleeNonce_Length) != 0)
		{
			WPS_PRINTF("Enrollee: Invalid Nonce, N1 in M2 is invalid!\n");
			return -1;
		}
		p += 2+2+E_EnrolleeNonce_Length;
	}

	//
	// Registrar Nonce
	//
	if (WPS_get_word(p) != WPS_h2n16(E_RegistrarNonce_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_RegistrarNonce_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		// Save Registrar Nonce
		WPS_MEM_CPY(enrollee->RegistrarNonce, p+4, E_RegistrarNonce_Length);
		p += 2+2+E_RegistrarNonce_Length;
	}

	//
	// UUID-R
	//
	if (WPS_get_word(p) != WPS_h2n16(E_UUID_R_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_UUID_R_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		// Save UUID_R
		WPS_MEM_CPY(enrollee->UUID_R, p+4, E_UUID_R_Length);
		p += 2+2+E_UUID_R_Length;
	}

	//
	// Registrar Public Key
	//
	if (WPS_get_word(p) != WPS_h2n16(E_PublicKey_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_PublicKey_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		// Save Registrar Public Key
		WPS_MEM_CPY(enrollee->PK_R, p+4, E_PublicKey_Length);
		p += 2+2+E_PublicKey_Length;
	}

	//
	// Authentication Type Flags
	//
	if (WPS_get_word(p) != WPS_h2n16(E_AuthenticationTypeFlags_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_AuthenticationTypeFlags_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		enrollee->AuthenticationTypeFlags_R = WPS_n2h16( WPS_get_word(p+4) );
		p += 2+2+E_AuthenticationTypeFlags_Length;
	}

	//
	// Encryption Type Flags
	//
	if (WPS_get_word(p) != WPS_h2n16(E_EncryptionTypeFlags_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_EncryptionTypeFlags_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		enrollee->EncryptionTypeFlags_R = WPS_n2h16( WPS_get_word(p+4) );
		p += 2+2+E_EncryptionTypeFlags_Length;
	}

	//
	// Connection Type Flags
	//
	if (WPS_get_word(p) != WPS_h2n16(E_ConnectionTypeFlags_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_ConnectionTypeFlags_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		enrollee->ConnectionTypeFlags_R = *(p+4);
		p += 2+2+E_ConnectionTypeFlags_Length;
	}

	//
	// Config Methods
	//
	if (WPS_get_word(p) != WPS_h2n16(E_ConfigMethods_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_ConfigMethods_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		enrollee->ConfigMethods_R = WPS_n2h16( WPS_get_word(p+4) );
		p += 2+2+E_ConfigMethods_Length;
	}

	//
	// Manufacturer
	//
	if (WPS_get_word(p) != WPS_h2n16(E_Manufacturer_ID)
		|| WPS_n2h16(WPS_get_word(p+2)) > E_Manufacturer_MaxLength)
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		DataLen = WPS_n2h16(WPS_get_word(p+2));
		WPS_MEM_CPY(enrollee->Manufacturer_R, p+4, DataLen);
		p += 2+2+DataLen;
	}

	//
	// Model Name
	//
	if (WPS_get_word(p) != WPS_h2n16(E_ModelName_ID)
		|| WPS_n2h16(WPS_get_word(p+2)) > E_ModelName_MaxLength)
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		DataLen = WPS_n2h16(WPS_get_word(p+2));
		WPS_MEM_CPY(enrollee->ModelName_R, p+4, DataLen);
		p += 2+2+DataLen;
	}

	//
	// Model Number
	//
	if (WPS_get_word(p) != WPS_h2n16(E_ModelNumber_ID)
		|| WPS_n2h16(WPS_get_word(p+2)) > E_ModelNumber_MaxLength)
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		DataLen = WPS_n2h16(WPS_get_word(p+2));
		WPS_MEM_CPY(enrollee->ModelNumber_R, p+4, DataLen);
		p += 2+2+DataLen;
	}

	//
	// Serial Number
	//
	if (WPS_get_word(p) != WPS_h2n16(E_SerialNumber_ID)
		|| WPS_n2h16(WPS_get_word(p+2)) > E_SerialNumber_MaxLength)
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else
	{
		DataLen = WPS_n2h16(WPS_get_word(p+2));
		WPS_MEM_CPY(enrollee->SerialNumber_R, p+4, DataLen);
		p += 2+2+DataLen;
	}

	//
	// Primary Device Type
	//
	if (WPS_get_word(p) != WPS_h2n16(E_PrimaryDeviceType_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_PrimaryDeviceType_Length))
	{
		WPS_PRINTF("Enrollee: Invalid Simple Config EAP packet...\n");
		enrollee->ConfigurationError_E = ERROR_MESSAGE_FORMAT_ERROR;
		wps_enrol_snd_nack(enrollee);
		return -1;
	}
	else