
#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <netinet/in.h>
#include "include/snmpv3AuthCry.h"

extern void md5_calc(
	unsigned char *output,
	unsigned char *input,								 /* input block */
	unsigned int inlen					   /* length of input block */
	);

extern void  des(unsigned char *output, const unsigned char *input, const unsigned char *key, int direction);

#define EXTEND_AUTH_LEN 64
#define MD5_DIGEST_LEN	16

#define MAX_SNMP_LEN	1500

#define	CRYPTIPN_BLOCK_LEN	8

static const u8 IPAD[EXTEND_AUTH_LEN]=
{
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36,
	0x36,0x36,0x36,0x36,0x36,0x36,0x36,0x36
};

static const u8 OPAD[EXTEND_AUTH_LEN]=
{
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,
	0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C,0x5C
};


static void XORFunction(u8 *input0, const u8 *input1,u8 *output,u8 len)
{
	int i;

	for(i=0;i<len;i++)
		output[i] = input0[i] ^ input1[i];
	return;
}

static long getCurrentTime()
{
	struct 		timeval tv;
    struct 		timezone tz;

	gettimeofday(&tv,&tz );

	return tv.tv_sec;
}

int snmpV3AuthenticateOutgoingMsg
(/*	result >0:success,else fail	*/
  u8 	*inAuthKey, 				/*secret key for authentication*/
  u16 	inAuthKeyLen,
  u8 	*inWholeMsg, 				/* unauthenticated complete message*/
  u16 	inWholeMsgLen,
  u8 	*outAuthParameters,    		/* authenticationParamemeter	*/
  u16 	*outAuthParametersLen
)
{
	int i;
	u8	extendedAuthKey[EXTEND_AUTH_LEN];
	u8	K1[EXTEND_AUTH_LEN],K2[EXTEND_AUTH_LEN];
	u8	snmpData[MAX_SNMP_LEN];
	u8	digest1[MD5_DIGEST_LEN],digest2[MD5_DIGEST_LEN];
		
		
	if(inAuthKeyLen != AUTH_KEY_LEN)
		return -1;

	memset(outAuthParameters,0,MD5_DIGEST_90_LEN);		
	memset(extendedAuthKey,0,EXTEND_AUTH_LEN);
	memcpy(extendedAuthKey,inAuthKey,AUTH_KEY_LEN);
	XORFunction(extendedAuthKey,IPAD,K1,EXTEND_AUTH_LEN);
	XORFunction(extendedAuthKey,OPAD,K2,EXTEND_AUTH_LEN);	

	memcpy(snmpData,K1,EXTEND_AUTH_LEN);
	memcpy(snmpData+EXTEND_AUTH_LEN,inWholeMsg,inWholeMsgLen);
	md5_calc(digest1,snmpData,EXTEND_AUTH_LEN+inWholeMsgLen);
		 
	memcpy(snmpData,K2,EXTEND_AUTH_LEN);
	memcpy(snmpData+EXTEND_AUTH_LEN,digest1,MD5_DIGEST_LEN);	
	md5_calc(digest2,snmpData,EXTEND_AUTH_LEN+MD5_DIGEST_LEN);
		 
	*outAuthParametersLen = MD5_DIGEST_90_LEN;
	for(i=0;i<MD5_DIGEST_90_LEN;i++)/*	take the first 12 octets	*/
		outAuthParameters[i] = digest2[i];	
	return 1;
}


int snmpV3AuthenticateIncomingMsg
(/*	result >0:success,else fail	*/
  u8 	*inAuthKey,					/*secret key for authentication*/
  u16 	inAuthKeyLen,
  u8 	*inWholeMsg, 				/* as received on the wire*/
  u16	inWholeMsgLen,
  u8 	*inAuthParameters, 			/* as received on the wire*/
  u16 	inAuthParametersLen
)
{
	int i;
	u8	extendedAuthKey[EXTEND_AUTH_LEN];
	u8	K1[EXTEND_AUTH_LEN],K2[EXTEND_AUTH_LEN];
	u8	snmpData[MAX_SNMP_LEN];
	u8	digest1[MD5_DIGEST_LEN],digest2[MD5_DIGEST_LEN],oldMAC[MD5_DIGEST_90_LEN],newMAC[MD5_DIGEST_90_LEN];

	if(inAuthKeyLen != AUTH_KEY_LEN)
		return -1;
	if(inAuthParametersLen != MD5_DIGEST_90_LEN)
		return -1;
	memcpy(oldMAC,inAuthParameters,MD5_DIGEST_90_LEN);
	memset(inAuthParameters,0,MD5_DIGEST_90_LEN);

	memset(extendedAuthKey,0,EXTEND_AUTH_LEN);
	memcpy(extendedAuthKey,inAuthKey,AUTH_KEY_LEN);
	XORFunction(extendedAuthKey,IPAD,K1,EXTEND_AUTH_LEN);
	XORFunction(extendedAuthKey,OPAD,K2,EXTEND_AUTH_LEN);

	memcpy(snmpData,K1,EXTEND_AUTH_LEN);
	memcpy(snmpData+EXTEND_AUTH_LEN,inWholeMsg,inWholeMsgLen);
	md5_calc(digest1,snmpData,EXTEND_AUTH_LEN+inWholeMsgLen);

	memcpy(snmpData,K2,EXTEND_AUTH_LEN);
	memcpy(snmpData+EXTEND_AUTH_LEN,digest1,MD5_DIGEST_LEN);	
	md5_calc(digest2,snmpData,EXTEND_AUTH_LEN+MD5_DIGEST_LEN);
	
	for(i=0;i<MD5_DIGEST_90_LEN;i++)/*	take the first 12 octets	*/
		newMAC[i] = digest2[i];	
	
	memcpy(inAuthParameters,oldMAC,MD5_DIGEST_90_LEN);
	if(memcmp(oldMAC,newMAC,MD5_DIGEST_90_LEN))
		return -1;
	return 1;
}

static void snmpV3CryptInitVector
(
 u32 	runTime,						/*	the running period	of the wxc2main*/
 u8		*snmpV3Salt
)
{
	ulong currentTime;
	static ulong count =0;
	//u8 inSalt[8]={0x00, 0x00, 0x00, 0xc8, 0x00,  0x00,  0x00,  0x21};
					
	memcpy(snmpV3Salt,&runTime,4);	
	currentTime = getCurrentTime() + count++;
	memcpy(snmpV3Salt+4,&currentTime,4);

	//memcpy(snmpV3Salt,inSalt,8);
	return;
}


int snmpV3EncryptData
(/*	result >0:success,else fail	*/
 u8 	*inEncryptKey, 				/* secret key for encryption*/
 u16 	inEncryptKeyLen,
 u8 	*inDataToEncrypt, 			/* data to encrypt (scopedPDU)*/
 u16 	inDataToEncryptLen,
 u8 	*outEncryptedData, 			/* encrypted data (encryptedPDU)*/
 u16 	*outEncryptedDataLen,
 u8 	*outPrivParameters, 		/* filled in by service provider*/
 u16 	*outPrivParametersLen,
 ulong	runTime
)
{
	int i,blockCount;
	u8	modValue,preIV[CRYPTIPN_BLOCK_LEN],iv[CRYPTIPN_BLOCK_LEN];
	u8	/*plainText[CRYPTIPN_BLOCK_LEN],*/XORedText[CRYPTIPN_BLOCK_LEN],CipherText[CRYPTIPN_BLOCK_LEN];
	u8	*pPlainText,*pCipherText;	
	u8  snmpV3Salt[CRYPTIPN_BLOCK_LEN];
	
	if(inEncryptKeyLen != AUTH_KEY_LEN)
		return -1;
	modValue = inDataToEncryptLen % CRYPTIPN_BLOCK_LEN;
	if(modValue)
	{/*	pad 0 to the rear	*/
		for(i=0;i<CRYPTIPN_BLOCK_LEN-modValue;i++)
			inDataToEncrypt[inDataToEncryptLen+i] = 0;
	}
	snmpV3CryptInitVector(runTime,snmpV3Salt);
	blockCount = modValue?(inDataToEncryptLen / CRYPTIPN_BLOCK_LEN)+1:(inDataToEncryptLen / CRYPTIPN_BLOCK_LEN);
	memcpy(preIV,inEncryptKey+CRYPTIPN_BLOCK_LEN,CRYPTIPN_BLOCK_LEN);
	XORFunction(snmpV3Salt,preIV,iv,CRYPTIPN_BLOCK_LEN);
	
	memcpy(CipherText,iv,CRYPTIPN_BLOCK_LEN);
	pPlainText = inDataToEncrypt;
	pCipherText = outEncryptedData;
	for(i=0;i<blockCount;i++)
	{
		XORFunction(pPlainText,CipherText,XORedText,CRYPTIPN_BLOCK_LEN);
		des(pCipherText,XORedText,inEncryptKey,0);
		pPlainText += CRYPTIPN_BLOCK_LEN;		
		memcpy(CipherText,pCipherText,CRYPTIPN_BLOCK_LEN);
		pCipherText += CRYPTIPN_BLOCK_LEN;				
	}
	*outEncryptedDataLen = CRYPTIPN_BLOCK_LEN*blockCount;
	memcpy(outPrivParameters,snmpV3Salt,CRYPTIPN_BLOCK_LEN);
	*outPrivParametersLen = CRYPTIPN_BLOCK_LEN;
	return 1;
}


int snmpV3DecryptData
(/*	result >0:success,else fail	*/
 u8 	*inDecryptKey, 				/*secret key for decryption*/
 u16 	inDecryptKeyLen,
 u8 	*inPrivParameters, 			/* as received on the wire*/
 u16 	inPrivParametersLen,
 u8 	*inEncryptedData,			/* encrypted data (encryptedPDU)*/
 u16	inEncryptedDataLen,
 u8 	*outDecryptedData, 			/*decrypted data (scopedPDU)*/
 u16	*outDecryptedDataLen
)
{
	int i,blockCount;
	u8	desKey[CRYPTIPN_BLOCK_LEN],preIV[CRYPTIPN_BLOCK_LEN];
	u8	CipherText[CRYPTIPN_BLOCK_LEN],/*XORedText[CRYPTIPN_BLOCK_LEN],*/previousCipherText[CRYPTIPN_BLOCK_LEN];
	u8	*pPlainText,*pCipherText;
	u8	initationVector[CRYPTIPN_BLOCK_LEN];
	
	if(inDecryptKeyLen != AUTH_KEY_LEN)
		return -1;
	if(inEncryptedDataLen % CRYPTIPN_BLOCK_LEN)
		return -1;
	blockCount = inEncryptedDataLen / CRYPTIPN_BLOCK_LEN;
	memcpy(desKey,inDecryptKey,CRYPTIPN_BLOCK_LEN);
	memcpy(preIV,inDecryptKey+CRYPTIPN_BLOCK_LEN,CRYPTIPN_BLOCK_LEN);
	XORFunction(preIV,inPrivParameters,initationVector,CRYPTIPN_BLOCK_LEN);		
	memcpy(previousCipherText,initationVector,CRYPTIPN_BLOCK_LEN);
	
	pPlainText = outDecryptedData;
	pCipherText = inEncryptedData;
	for(i=0;i<blockCount;i++)
	{
		des(CipherText,pCipherText,desKey,1);		
		XORFunction(CipherText,previousCipherText,pPlainText,CRYPTIPN_BLOCK_LEN);		
		pPlainText += CRYPTIPN_BLOCK_LEN;
		memcpy(previousCipherText,pCipherText,CRYPTIPN_BLOCK_LEN);
		pCipherText += CRYPTIPN_BLOCK_LEN;				
	}
	*outDecryptedDataLen = inEncryptedDataLen;
	return 1;	
}














