wps_enrollee.c

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

	{
		WPS_MEM_CPY((WPS_u8 *)(&(enrollee->primary_device_type_R)), p+4, E_PrimaryDeviceType_Length);
		p += 2+2+E_PrimaryDeviceType_Length;
	}

	//
	// Device Name
	//
	if (WPS_get_word(p) != WPS_h2n16(E_DeviceName_ID)
		|| WPS_n2h16(WPS_get_word(p+2)) > E_DeviceName_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->DeviceName_R, p+4, DataLen);
		p += 2+2+DataLen;
	}

	//
	// RF Bands
	//
	if (WPS_get_word(p) != WPS_h2n16(E_RFBands_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_RFBands_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->RFBands_R = *(p+4);
		p += 2+2+E_RFBands_Length;
	}

	//
	// Association State
	//
	if (WPS_get_word(p) != WPS_h2n16(E_AssociationState_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_AssociationState_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->AssociationState_R = WPS_n2h16( WPS_get_word(p+4) );
		p += 2+2+E_AssociationState_Length;
	}

	//
	// Configuration Error
	//
	if (WPS_get_word(p) != WPS_h2n16(E_ConfigurationError_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_ConfigurationError_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->ConfigurationError_R = WPS_n2h16( WPS_get_word(p+4) );
		p += 2+2+E_ConfigurationError_Length;
	}

	//
	// Device Password ID
	//
	if (WPS_get_word(p) != WPS_h2n16(E_DevicePasswordID_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_DevicePasswordID_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->DevicePasswordID_R = WPS_n2h16( WPS_get_word(p+4) );
		p += 2+2+E_DevicePasswordID_Length;
	}

	//
	// OS Version
	//
	if (WPS_get_word(p) != WPS_h2n16(E_OSVersion_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_OSVersion_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->OSVersion_R = WPS_n2h32( WPS_get_dword(p+4) );
		p += 2+2+E_OSVersion_Length;
	}

	//
	// Some optional elements...
	//
	while (WPS_get_word(p) != WPS_h2n16(E_Authenticator_ID))
	{
		// To do...
		p += 2+2+WPS_n2h16(WPS_get_word(p+2));
	}

	//
	// Compute AuthKey, KeyWrapKey, EMSK
	// 
	wps_enrol_key_derivation(enrollee);

	//
	// Authenticator
	//
	if (WPS_get_word(p) != WPS_h2n16(E_Authenticator_ID)
		|| WPS_get_word(p+2) != WPS_h2n16(E_Authenticator_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
	{
		WPS_u8 Output[32];
		WPS_s16 InputLen = enrollee->LastMessageLength+EAP_MESSAGE_LENGTH(packet) - (2+2+E_Authenticator_Length);
		WPS_u8 *Input = (WPS_u8 *)WPS_MALLOC(InputLen);
		WPS_MEM_CPY(Input, enrollee->LastMessage, enrollee->LastMessageLength);
		WPS_MEM_CPY((Input+enrollee->LastMessageLength), (WPS_u8 *)packet+EAP_HEAD_LENGTH, EAP_MESSAGE_LENGTH(packet) - (2+2+E_Authenticator_Length));
		WPS_HMAC_Sha256(enrollee->AuthKey, 32, Input, InputLen, Output, 32);
		WPS_MFREE(Input);

		if (WPS_MEM_CMP(p+4, Output, E_Authenticator_Length) != 0)
		{
			WPS_PRINTF("Enrollee: Invalid Authenticator when receiving M2\n");
			return -1;
		}

		p += 2+2+E_Authenticator_Length;
	}

	//
	// First, compute PSK1/PSK2, EHash1/EHash2
	//
	WPS_s32 Input1_Len;
	WPS_s32 Input2_Len;
	WPS_u8 *Input1;
	WPS_u8 *Input2;
	WPS_u8 PSK1[32]; //Only use 16 bytes
	WPS_u8 PSK2[32]; //Only use 16 bytes
	if ((enrollee->DevicePassword_Length % 2) == 0)
	{
		Input1_Len = enrollee->DevicePassword_Length / 2;
		Input2_Len = Input1_Len;
	}
	else
	{
		Input1_Len = (enrollee->DevicePassword_Length+1) / 2;
		Input2_Len = Input1_Len - 1;
	}
	Input1 = enrollee->DevicePassword;
	Input2 = enrollee->DevicePassword+Input1_Len;

	WPS_HMAC_Sha256(enrollee->AuthKey, 32, Input1, Input1_Len, PSK1, 32);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, Input2, Input2_Len, PSK2, 32);

	// Save the two 16 bytes PSK
	WPS_MEM_CPY(enrollee->PSK1, PSK1, 16);
	WPS_MEM_CPY(enrollee->PSK2, PSK2, 16);

	WPS_u8 TempInput1[ E_ESNonce1_Length+16+E_PublicKey_Length+E_PublicKey_Length ];
	WPS_MEM_CPY(TempInput1, enrollee->ESNonce1, E_ESNonce1_Length);
	WPS_MEM_CPY(TempInput1+E_ESNonce1_Length, PSK1, 16);
	WPS_MEM_CPY(TempInput1+E_ESNonce1_Length+16, enrollee->PK_E, E_PublicKey_Length);
	WPS_MEM_CPY(TempInput1+E_ESNonce1_Length+16+E_PublicKey_Length, enrollee->PK_R, E_PublicKey_Length);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, TempInput1, E_ESNonce1_Length+16+E_PublicKey_Length+E_PublicKey_Length, enrollee->EHash1, E_EHash1_Length);

	WPS_u8 TempInput2[ E_ESNonce2_Length+16+E_PublicKey_Length+E_PublicKey_Length ];
	WPS_MEM_CPY(TempInput2, enrollee->ESNonce2, E_ESNonce2_Length);
	WPS_MEM_CPY(TempInput2+E_ESNonce2_Length, PSK2, 16);
	WPS_MEM_CPY(TempInput2+E_ESNonce2_Length+16, enrollee->PK_E, E_PublicKey_Length);
	WPS_MEM_CPY(TempInput2+E_ESNonce2_Length+16+E_PublicKey_Length, enrollee->PK_R, E_PublicKey_Length);
	WPS_HMAC_Sha256(enrollee->AuthKey, 32, TempInput2, E_ESNonce2_Length+16+E_PublicKey_Length+E_PublicKey_Length, enrollee->EHash2, E_EHash2_Length);

	//
	// Update LastMessage point
	//
	if (wps_enrol_upt_last_msg_for_rcv(enrollee, packet) == -1)
	{
		return -1;
	}

	return wps_enrol_snd_m3(enrollee);
}

WPS_s32 wps_enrol_rcv_m2d(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 M2D\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("OnReceiveM2D: Enrollee state change to E_STATE_E\n");
#endif


#if ENROLLEE_CHECK_M2D

	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 M2D 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;
	}

	//
	// 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 C