wps_lib.c

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

/*
 *  WPS_LIB.C : WPS Enrollee/Registrar Common Functions Lib
 * 
 *  ver       date            author        comment
 *  0.0.1     06/08/28        Gao Hua       First
 *  0.0.2     07/09/05        D.W.Yan       Refactor code
 */

#include "wps_types.h"
#include "wps_common.h"
#include "wps_osfunc.h"
#include "wps_crypto.h"
#include "wps_eap_pkt.h"
#include "wps_data_element.h"

#include "wps_lib.h"


WPS_char g_sz1356_p[] =
"FFFFFFFFFFFFFFFFC90FDAA22168C234C4C6628B80DC1CD1"
"29024E088A67CC74020BBEA63B139B22514A08798E3404DD"
"EF9519B3CD3A431B302B0A6DF25F14374FE1356D6D51C245"
"E485B576625E7EC6F44C42E9A637ED6B0BFF5CB6F406B7ED"
"EE386BFB5A899FA5AE9F24117C4B1FE649286651ECE45B3D"
"C2007CB8A163BF0598DA48361C55D39A69163FA8FD24CF5F"
"83655D23DCA3AD961C62F356208552BB9ED529077096966D"
"670C354E4ABC9804F1746C08CA237327FFFFFFFFFFFFFFFF";

WPS_char g_sz1356_g[] = "02";


/*
 *  kdf(key, personalization_string, total_key_bits) :
 *  result := ""
 *  iterations = (total_key_bits + prf_digest_size - 1)/prf_digest_size
 *  for i = 1 to iterations do
 *  result := result || prf(key, i || personalization_string || total_key_bits)
 *  return 1st total_key_bits of result and destroy any bits left over
 *  */
void kdf(WPS_u8 *KDK, WPS_s32 KDKLen, WPS_char *string, WPS_u8 *Output,
         WPS_s32 total_key_bits)
{
	WPS_s32 i;
	WPS_s32 iterations = (total_key_bits+256-1)/256;
	WPS_s32 InputLen = sizeof(i)+WPS_STR_LEN(string)+sizeof(total_key_bits);
	WPS_u8 *Input = (WPS_u8 *)WPS_MALLOC(InputLen);
	WPS_MEM_FILL(Output, 0, total_key_bits/8);
	for (i=1; i<=iterations; i++)
	{		
		 /* Concatenation is big endian:
		  * Low Address ----------------------------------> Heigh Address
		  * i4 | i3 | i2 | i1 | s0 | s1 | s2 | ... sN | t4 | t3 | t2 | t1
		  * */ 
		
		*(WPS_s32 *)Input = WPS_h2n32(i);
		WPS_STR_CPY((WPS_char *)(Input+sizeof(i)), string);
		*(WPS_s32 *)(Input+sizeof(i)+WPS_STR_LEN(string))
		= WPS_h2n32(total_key_bits);
		if (32*i > total_key_bits/8)
		{
			/* Maybe Output buffer size is less than 32*iterations */
			WPS_HMAC_Sha256(KDK, KDKLen, Input, InputLen, Output+32*(i-1),
			            (total_key_bits/8-32*(i-1)));
		}
		else
		{
			WPS_HMAC_Sha256(KDK, KDKLen, Input, InputLen, Output+32*(i-1), 32);
		}
	}
	WPS_MFREE(Input);
}

/* Return values:
 *  1: Checksum OK
 *  0: Checksum failed
 * */
WPS_u8 ValidateChecksum(WPS_u32 PIN)
{
	WPS_u32 accum = 0;
	accum += 3 * ((PIN / 10000000) % 10);
	accum += 1 * ((PIN / 1000000) % 10);
	accum += 3 * ((PIN / 100000) % 10);
	accum += 1 * ((PIN / 10000) % 10);
	accum += 3 * ((PIN / 1000) % 10);
	accum += 1 * ((PIN / 100) % 10);
	accum += 3 * ((PIN / 10) % 10);
	accum += 1 * ((PIN / 1) % 10);
	if (0 == (accum % 10)) 	return 1;
	else                    return 0;
}

WPS_s32 ComputeChecksum(WPS_u32 PIN)
{
	WPS_u32 accum = 0;
	PIN *= 10;
	accum += 3 * ((PIN / 10000000) % 10);
	accum += 1 * ((PIN / 1000000) % 10);
	accum += 3 * ((PIN / 100000) % 10);
	accum += 1 * ((PIN / 10000) % 10);
	accum += 3 * ((PIN / 1000) % 10);
	accum += 1 * ((PIN / 100) % 10);
	accum += 3 * ((PIN / 10) % 10);
	WPS_s32 digit = (accum % 10);
	return (10-digit) % 10;
}

void Generate_PIN(WPS_u8 *MacAddr, WPS_char *Buff, WPS_s32 BuffLen)
{
	if (BuffLen < 9) {
		WPS_PRINTF("WPS_Get_PIN: PIN buff is smaller than 9 bytes");
		return;
	}

	WPS_char tempBuff[9];
	WPS_u32 PIN;
	WPS_s32 last;

	WPS_u32 n = WPS_get_dword(MacAddr+2);
	n = WPS_n2h32(n);
	n = n & 0x007fffff; /* only use the lower 23 bits, 0x007fffff = 8388607 */
	last = ComputeChecksum(n);
	PIN = n*10+last;
	WPS_MEM_FILL(tempBuff, 0, 9);
	WPS_dwtoa(PIN, tempBuff);
	WPS_STR_CPY(Buff, "00000000");
	WPS_STR_CPY(Buff+8-WPS_STR_LEN(tempBuff), tempBuff);
}

void Generate_UUID(WPS_u8 *MacAddr, WPS_u8 *Buff, WPS_s32 BuffLen) /*  see ../pbox/upnpd.c  */
{
	if (BuffLen < 16) {
		WPS_PRINTF("WPS_Get_UUID: UUID buff is smaller than MD5_DIGEST_LENGTH");
		return;
	}

	WPS_u8 temp[16];
	
	WPS_Md5(MacAddr, 6, temp, 16);	
	temp[6] &= 0x0f;
	temp[6] |= 0x30; /* Version=3 */
	temp[8] &= 0x3f;
	temp[8] |= 0x80; /* Variants */
	
	WPS_MEM_CPY(Buff, temp, 16);
}

/*
 void BuildEAPHead(struct eap_packet* packet, WPS_u8 Code, WPS_u8 Identifier,
                   WPS_u8 OpCode, WPS_s32 MessageLength)
 {
 WPS_u8 *p = (WPS_u8*)packet;
 *(p+0) = Code;
 *(p+1) = Identifier;
 WPS_SET_WORD(p+2, WPS_h2n16(EAP_HEAD_LENGTH+MessageLength));
 WPS_SET_DWORD(p+4, WPS_h2n32(((dword)WPS_EXPANDED_TYPE<<24) | WPS_VENDOR_ID));

 WPS_SET_DWORD(p+8, WPS_h2n32(WPS_VENDOR_TYPE));
 *(p+12) = OpCode;
 *(p+13) = FLAGS_LF;
 WPS_SET_WORD(p+14, WPS_h2n16(MessageLength));

 WPS_s32 i;
 for (i=0; i<(EAP_HEAD_LENGTH+MessageLength); i++)
 {
 if (i%16 == 0)
 WPS_PRINTF("\n");		
 WPS_PRINTF("%x ", p[i]);			
 }
 WPS_PRINTF("\n");
 
 }
 */

void BuildEAPHead(struct eap_packet* packet, WPS_u8 Code, WPS_u8 Identifier,
                  WPS_u8 OpCode, WPS_s32 MessageLength)
{
	WPS_u8 *p = (WPS_u8*)packet;

	packet->Code = Code;
	packet->Identifier = Identifier;
	packet->Length = WPS_h2n16(EAP_HEAD_LENGTH+MessageLength);

	WPS_set_dword(p+4, WPS_h2n32(((WPS_u32)WPS_EXPANDED_TYPE<<24) | WPS_VENDOR_ID));

	WPS_set_dword(p+8, WPS_h2n32(WPS_VENDOR_TYPE));

	packet->OpCode = OpCode;
	packet->Flags = 0;
}

/*
 void PrintHex(WPS_u8 *Hex, WPS_s32 HexLen)
 {
 WPS_s32 i;

 for (i=0; i<HexLen; i++)
 {
 if (i%8 == 0)
 {
 WPS_PRINTF("\n");
 }
 WPS_PRINTF("0x%02x ", *(Hex+i));
 }
 WPS_PRINTF("\n\n");
 }
 */

/* Assume HexLen = 8*n */
void PrintHex(WPS_u8 *Hex, WPS_s32 HexLen)
{
	WPS_s32 i;

	for (i=0; i<HexLen; i+=8) {
		WPS_PRINTF("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x ", *(Hex+i), *(Hex
		    +i+1), *(Hex+i+2), *(Hex+i+3), *(Hex+i+4), *(Hex+i+5), *(Hex+i+6),
		           *(Hex+i+7));
	}
	WPS_PRINTF("\n");
}

WPS_char *WPS_wtoa(WPS_u16 w, WPS_char *buff)
{
#if defined(WPS_COMMON_LIBC)
	if (buff == NULL) return NULL;
	sprintf(buff, "%d", w);
	return buff;
#elif defined(WPS_COMMON_CREAM)
	return wtoa(w, buff);
#endif
}

WPS_char *WPS_dwtoa(WPS_u32 dw, WPS_char *buff)
{
#if defined(WPS_COMMON_LIBC)
	if (buff == NULL) return NULL;
	sprintf(buff, "%ld", dw);
	return buff;
#elif defined(WPS_COMMON_CREAM)
	return dwtoa(dw, buff);
#endif
}

WPS_char *WPS_btox(WPS_u8 b, WPS_char *ptr)
{
#if defined(WPS_COMMON_LIBC)
	if (ptr == NULL) return NULL;
	sprintf(ptr, "%02x", b);
	return (ptr);
#elif defined(WPS_COMMON_CREAM)
	return btox(b, ptr);
#endif
}