#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#ifndef WIN32
#include <unistd.h>
#endif

#include <errno.h>
#include <ctype.h>

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#define DMID 15
#include "debug.h"
#include "md5.h"
#include "lib.h"

#define F(x, y, z) (((x) & (y)) | ((~x) & (z)))
#define G(x, y, z) (((x) & (z)) | ((y) & (~z)))
#define H(x, y, z) ((x) ^ (y) ^ (z))
#define I(x, y, z) ((y) ^ ((x) | (~z)))

#define CROT(x, y) (((x) << (y)) | ((x) >> (32-(y))))

#define FF(a1, b1, c1, d1, k1, s1, i1) a1=(b1)+CROT(((a1) + F((b1), (c1), (d1)) + x[k1] + T[i1]), (s1))
#define GG(a1, b1, c1, d1, k1, s1, i1) a1=(b1)+CROT(((a1) + G((b1), (c1), (d1)) + x[k1] + T[i1]), (s1))
#define HH(a1, b1, c1, d1, k1, s1, i1) a1=(b1)+CROT(((a1) + H((b1), (c1), (d1)) + x[k1] + T[i1]), (s1))
#define II(a1, b1, c1, d1, k1, s1, i1) a1=(b1)+CROT(((a1) + I((b1), (c1), (d1)) + x[k1] + T[i1]), (s1))

unsigned int T[65]=
{
	0x00000000,
	0xd76aa478,0xe8c7b756,0x242070db,0xc1bdceee,0xf57c0faf,0x4787c62a,0xa8304613,0xfd469501,
	0x698098d8,0x8b44f7af,0xffff5bb1,0x895cd7be,0x6b901122,0xfd987193,0xa679438e,0x49b40821,
	0xf61e2562,0xc040b340,0x265e5a51,0xe9b6c7aa,0xd62f105d,0x02441453,0xd8a1e681,0xe7d3fbc8,
	0x21e1cde6,0xc33707d6,0xf4d50d87,0x455a14ed,0xa9e3e905,0xfcefa3f8,0x676f02d9,0x8d2a4c8a,
	0xfffa3942,0x8771f681,0x6d9d6122,0xfde5380c,0xa4beea44,0x4bdecfa9,0xf6bb4b60,0xbebfbc70,
	0x289b7ec6,0xeaa127fa,0xd4ef3085,0x04881d05,0xd9d4d039,0xe6db99e5,0x1fa27cf8,0xc4ac5665,
	0xf4292244,0x432aff97,0xab9423a7,0xfc93a039,0x655b59c3,0x8f0ccc92,0xffeff47d,0x85845dd1,
	0x6fa87e4f,0xfe2ce6e0,0xa3014314,0x4e0811a1,0xf7537e82,0xbd3af235,0x2ad7d2bb,0xeb86d391
};
char *mlib_bin_to_hex(char *bin, int binlen, char *out, int outlen);

void md5_transform(unsigned int state[6], unsigned char block[64])
{
	unsigned int A=state[0], B=state[1], C=state[2], D=state[3], x[16];
	unsigned int i;

	for(i=0;i<64;i+=4)
		x[i/4]=(block[i+0]<<0)|(block[i+1]<<8)|(block[i+2]<<16)|(block[i+3]<<24);

	FF(A, B, C, D, 0, 7, 1);
	FF(D, A, B, C, 1, 12, 2);
	FF(C, D, A, B, 2, 17, 3);
	FF(B, C, D, A, 3, 22, 4);

	FF(A, B, C, D, 4, 7, 5);
	FF(D, A, B, C, 5, 12, 6);
	FF(C, D, A, B, 6, 17, 7);
	FF(B, C, D, A, 7, 22, 8);

	FF(A, B, C, D, 8, 7, 9);
	FF(D, A, B, C, 9, 12, 10);
	FF(C, D, A, B, 10, 17, 11);
	FF(B, C, D, A, 11, 22, 12);

	FF(A, B, C, D, 12, 7, 13);
	FF(D, A, B, C, 13, 12, 14);
	FF(C, D, A, B, 14, 17, 15);
	FF(B, C, D, A, 15, 22, 16);

	GG(A, B, C, D, 1, 5, 17);
	GG(D, A, B, C, 6, 9, 18);
	GG(C, D, A, B, 11, 14, 19);
	GG(B, C, D, A, 0, 20, 20);

	GG(A, B, C, D, 5, 5, 21);
	GG(D, A, B, C, 10, 9, 22);
	GG(C, D, A, B, 15, 14, 23);
	GG(B, C, D, A, 4, 20, 24);

	GG(A, B, C, D, 9, 5, 25);
	GG(D, A, B, C, 14, 9, 26);
	GG(C, D, A, B, 3, 14, 27);
	GG(B, C, D, A, 8, 20, 28);

	GG(A, B, C, D, 13, 5, 29);
	GG(D, A, B, C, 2, 9, 30);
	GG(C, D, A, B, 7, 14, 31);
	GG(B, C, D, A, 12, 20, 32);

	HH(A, B, C, D, 5, 4, 33);
	HH(D, A, B, C, 8, 11, 34);
	HH(C, D, A, B, 11, 16, 35);
	HH(B, C, D, A, 14, 23, 36);

	HH(A, B, C, D, 1, 4, 37);
	HH(D, A, B, C, 4, 11, 38);
	HH(C, D, A, B, 7, 16, 39);
	HH(B, C, D, A, 10, 23, 40);

	HH(A, B, C, D, 13, 4, 41);
	HH(D, A, B, C, 0, 11, 42);
	HH(C, D, A, B, 3, 16, 43);
	HH(B, C, D, A, 6, 23, 44);

	HH(A, B, C, D, 9, 4, 45);
	HH(D, A, B, C, 12, 11, 46);
	HH(C, D, A, B, 15, 16, 47);
	HH(B, C, D, A, 2, 23, 48);

	II(A, B, C, D, 0, 6, 49);
	II(D, A, B, C, 7, 10, 50);
	II(C, D, A, B, 14, 15, 51);
	II(B, C, D, A, 5, 21, 52);

	II(A, B, C, D, 12, 6, 53);
	II(D, A, B, C, 3, 10, 54);
	II(C, D, A, B, 10, 15, 55);
	II(B, C, D, A, 1, 21, 56);

	II(A, B, C, D, 8, 6, 57);
	II(D, A, B, C, 15, 10, 58);
	II(C, D, A, B, 6, 15, 59);
	II(B, C, D, A, 13, 21, 60);

	II(A, B, C, D, 4, 6, 61);
	II(D, A, B, C, 11, 10, 62);
	II(C, D, A, B, 2, 15, 63);
	II(B, C, D, A, 9, 21, 64);

	state[0]+=A;
	state[1]+=B;
	state[2]+=C;
	state[3]+=D;
} 

void md5_set_state(unsigned int state[6], unsigned char digest[16])
{
	unsigned int i;

	for(i=0;i<4;i++)
	{
		state[i] =	digest[(4*i)+0]<< 0 |
					digest[(4*i)+1]<< 8 |
					digest[(4*i)+2]<<16 |
					digest[(4*i)+3]<<24;
	}

	state[4] = state[5] = 0;
}

void md5_get_state(unsigned int state[6], unsigned char digest[16])
{
	unsigned int i;

	for(i=0;i<4;i++)
	{
		digest[(4*i)+0]=(state[i]>>0)&0xFF;
		digest[(4*i)+1]=(state[i]>>8)&0xFF;
		digest[(4*i)+2]=(state[i]>>16)&0xFF;
		digest[(4*i)+3]=(state[i]>>24)&0xFF;
	}
}

void md5_init(unsigned int state[6])
{
	state[0]=0x67452301;
	state[1]=0xefcdab89;
	state[2]=0x98badcfe;
	state[3]=0x10325476;
	state[4]=0;
	state[5]=0;
}

void md5_update(unsigned int state[6], char *data, unsigned int datalen, unsigned char *buffer)
{
	unsigned int i, index, partLen;

	index=state[4]&0x3F;

	state[4]+=datalen;

	partLen=64-index;

	if(datalen>=partLen)
	{
		memcpy(&(buffer[index]), data, partLen);

		md5_transform(state, buffer);

		for(i=partLen;i+63<datalen;i+= 64)
			md5_transform(state, &data[i]);

		index=0;
	}
	else
		i=0;

	/* Buffer remaining data */
	memcpy(&(buffer[index]), &(data[i]), datalen-i);
}

void md5_finish(unsigned int state[6], unsigned char buffer[64], unsigned char digest[16])
{
	unsigned int index, padLen, bc1, bc2;
	unsigned char padding[64];
	unsigned char bits[8];

	memset(padding, 0, 64);
	padding[0]=0x80;

	bc1=state[4]&0x1FFFFFFF;
	bc1<<=3;

	bc2=state[4]&0xE0000000;
	bc2>>=26;

	bits[0]=(bc1>>0)&0xFF;
	bits[1]=(bc1>>8)&0xFF;
	bits[2]=(bc1>>16)&0xFF;
	bits[3]=(bc1>>24)&0xFF;
	bits[4]=(bc2>>0)&0xFF;
	bits[5]=(bc2>>8)&0xFF;
	bits[6]=(bc2>>16)&0xFF;
	bits[7]=(bc2>>24)&0xFF;

	index=state[4]&0x3f;
	padLen=(index<56)?(56-index):(120 - index);
  
	md5_update(state, padding, padLen, buffer);
	md5_update(state, bits, 8, buffer);

	md5_get_state(state,digest);
}

void md5_digest(char *data, unsigned int datalen, unsigned char digest[16])
{
	unsigned char buffer[64];
	unsigned int state[6];

	md5_init(state);
	md5_update(state,data,datalen,buffer);
	md5_finish(state,buffer,digest);
}

void md5_print(unsigned char digest[16], char *buf, int bfsz)
{
	int i,max;
	max = bfsz-1;
	if (max > 32)
		max = 32;

	for(i = 0; i < max; ) {
		buf[i] = "0123456789ABCDEF"[digest[i/2] / 16];
		i++;
		buf[i] = "0123456789ABCDEF"[digest[i/2] % 16];
		i++;
	}
	buf[i] = 0;
}
void md5_print2(unsigned char digest[], char *buf, int max)
{
	int i;

	for(i = 0; i < max; ) {
		buf[i] = "0123456789ABCDEF"[digest[i/2] / 16];
		i++;
		buf[i] = "0123456789ABCDEF"[digest[i/2] % 16];
		i++;
	}
	buf[i] = 0;

}

int md5_file(char *fname,char *md5)
{
	char bf[BFSZ];
	int want;
	unsigned char buffer[64];
	unsigned int ctx[6];
    unsigned char md5sum[BFSZ];
	FILE *f;
	strcpy(md5,"");
	f = fopen(fname,"rb");
	if (f==NULL) return FALSE;
    md5_init( ctx );
	  for (;!feof(f);) {
		int r;
		want = BFSZ-1;
		r = (int)fread(bf,1,want,f);
		if (r<=0) break;
		md5_update( ctx, bf, r ,buffer);
	  }
    md5_finish( ctx, buffer,md5sum);
	fclose(f);
	mlib_bin_to_hex(md5sum,16,md5,BFSZ-1);
	return TRUE;
}

void md5_unprint(unsigned char digest[16], char *buf)
{
	int i;

	for(i = 0; i < 16; i++) {
		digest[i]  = (isdigit(buf[(i*2)]) ? buf[(i*2)]-'0' : (buf[(i*2)]^0x20)-'a'+10) << 4;
		digest[i] += isdigit(buf[(i*2)+1]) ? buf[(i*2)+1]-'0' : (buf[(i*2)+1]^0x20)-'a'+10;
	}
}

void md5_hmac_create(char *key, unsigned int keylen, char *ipad, char *opad, int bfsz)
{
	unsigned char buffer[64];
	unsigned int state[6];
	unsigned char tmp[16];

	unsigned char tkey[16];

	unsigned char xipad[64];
	unsigned char xopad[64];
	char keypad[64];
	unsigned int i;
	
	if (keylen>64) {
		md5_digest(key,keylen,tkey);
		keylen = 16;
		key = tkey;
	}

	for(i=0;i<keylen;i++)
		keypad[i]=key[i];
	for(;i<64;i++)
		keypad[i]=0;

	for(i=0;i<64;i++) {
		xipad[i]=0x36^keypad[i];
		xopad[i]=0x5C^keypad[i];
	}

	md5_init(state);
	md5_update(state,xipad,64,buffer);
	md5_get_state(state,tmp);

	md5_print(tmp,ipad,bfsz);

	md5_init(state);
	md5_update(state,xopad,64,buffer);
	md5_get_state(state,tmp);
	md5_print(tmp,opad,bfsz);

}

void md5_hmac(char *data, unsigned int datalen, char *ipad, char *opad, unsigned char digest[16])
{
	unsigned char buffer[64];
	unsigned int state[6];
	unsigned char one[16];
	unsigned char tmp[16];

	md5_unprint(tmp,ipad);
	md5_set_state(state,tmp); state[4] = 64;
	md5_update(state,data,datalen,buffer);
	md5_finish(state,buffer,one);

	md5_unprint(tmp,opad);
	md5_set_state(state,tmp); state[4] = 64;
	md5_update(state,one,16,buffer);
	md5_finish(state,buffer,digest);
}

void md5_keyed(char *data, unsigned int datalen, char *key, unsigned int keylen, unsigned char digest[16])
{
	char keypad[64], *xipad, *xopad;
	unsigned char tkey[16];
	unsigned int i;

	if (keylen>64) {
		md5_digest(key,keylen,tkey);
		keylen = 16;
		key = tkey;
	}

	for(i=0;i<keylen;i++)
		keypad[i]=key[i];
	for(;i<64;i++)
		keypad[i]=0;

	xipad=calloc(64+datalen,1);
	for(i=0;i<64;i++)
		xipad[i]=0x36^keypad[i];
	for(i=0;i<datalen;i++)
		xipad[i+64]=data[i];

	md5_digest(xipad, datalen+64, tkey);
	{
		unsigned int state[6];
		unsigned char buffer[64];

		md5_init(state);
		md5_update(state,xipad,64,buffer);
		md5_update(state,data,datalen,buffer);
		md5_finish(state,buffer,digest);
	}
	

	xopad=calloc(64+16, 1);
	for(i=0;i<64;i++)
		xopad[i]=0x5C^keypad[i];
	for(i=0;i<16;i++)
		xopad[i+64]=digest[i];

	md5_digest(xopad, 64+16, tkey);
	{
		unsigned int state[6];
		unsigned char buffer[64];

		md5_init(state);
		md5_update(state,xopad,64,buffer);
		md5_update(state,digest,16,buffer);
		md5_finish(state,buffer,digest);
	}

	free(xipad);
	free(xopad);
}

static char* testStrings[] = {
	"",
	"a",
	"abc",
	"message digest",
	"abcdefghijklmnopqrstuvwxyz",
	"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789",
	"12345678901234567890123456789012345678901234567890123456789012345678901234567890",
	NULL
};
static char* testResults[] = {
	"D41D8CD98F00B204E9800998ECF8427E",
	"0CC175B9C0F1B6A831C399E269772661",
	"900150983CD24FB0D6963F7D28E17F72",
	"F96B697D7CB7938D525A2F31AAF161D0",
	"C3FCD3D76192E4007DFB496CCA67E13B",
	"D174AB98D277D9F5A5611C2C9F419D9F",
	"57EDF4A22BE3C955AC49DA2E2107B67A"
};

static char *key = "tanstaaftanstaaf";
static char *key_data = "<1896.697170952@postoffice.reston.mci.net>";
static char *key_result = "B913A602C7EDA7A495B4E6E7334D3890";

int md5_main(int argc, char **argv)
{
	char digest[16], buf[33], ipad[33], opad[33];
	int i;

	for(i = 0; testStrings[i]; i++) {
		md5_digest(testStrings[i],(int)strlen(testStrings[i]),digest);
		md5_print(digest,buf,33);
		if (strcmp(buf,testResults[i]) != 0)
			printf("MD5: test %d failed\n",i);
	}

	md5_keyed(key_data,(int)strlen(key_data),key,(int)strlen(key),digest);
	md5_print(digest,buf,33);
	if (strcmp(buf,key_result) != 0)
		printf("MD5: keyed test failed\n");

	md5_hmac_create(key,(int)strlen(key),ipad,opad,33);
	md5_hmac(key_data,(int)strlen(key_data),ipad,opad,digest);
	md5_print(digest,buf,33);
	if (strcmp(buf,key_result) != 0)
		printf("MD5: hmac test failed\n");

	return 0;
}
#define BFSZ 1000
char *mlib_bin_to_hex(char *bin, int binlen, char *out, int outlen)
{
	unsigned char *ubin=(unsigned char *) bin;
	int i;
	int z;
	char *out_start = out;
	for (i=0; i<binlen; i++,ubin++)  {
	      sprintf(out,"%02x",(unsigned int) *ubin);
	      z = (int)strlen(out);
	      out += z;
	      outlen -= z;
	      if (outlen<20) break;
	 }
	*out = 0;
	 return out_start;
}
int hmac_test(void)
{
	char bf[BFSZ];
	char digest[BFSZ];
	char *key =         "Jefe";
	char *data =        "what do ya want for nothing?";
	char *digest_text =      "750c783e6ab0b503eaa86e310a5db738";



	hmac_md5(data,(int)strlen(data),key,(int)strlen(key),digest);
	mlib_bin_to_hex(digest,16,bf,BFSZ);
//	printf("Hex (%s)\n",bf);
//	printf("Hex (%s)\n",digest_text);
	if (strcmp(bf,digest_text)!=0) printf("HMAC_TEST FAILED\n");
	return TRUE;
}

typedef unsigned int MD5_CTX[6];
void hmac_md5(unsigned char *text, int text_len, unsigned char *key, int key_len, char *digest)
{
        unsigned int context[6];
		unsigned char buffer[64];
        unsigned char k_ipad[65];    
        unsigned char k_opad[65];    
        unsigned char tk[16];
        int i;
        if (key_len > 64) {
                unsigned int tctx[6];
                md5_init(tctx);
                md5_update(tctx, key, key_len,buffer);
                md5_finish(tctx,buffer,tk);
                key = tk;
                key_len = 16;
        }

       /* start out by storing key in pads */
        memset( k_ipad, 0,sizeof k_ipad);
        memset( k_opad, 0,sizeof k_opad);
        memcpy( k_ipad, key, key_len);
        memcpy( k_opad, key, key_len);

        /* XOR key with ipad and opad values */
        for (i=0; i<64; i++) {
                k_ipad[i] ^= 0x36;
                k_opad[i] ^= 0x5c;
        }
        md5_init(context);                   /* init context for 1st */
        md5_update(context, k_ipad, 64,buffer);     /* start with inner pad */
        md5_update(context, text, text_len,buffer); /* then text of datagram */
        md5_finish(context,buffer,digest);

        md5_init(context);                   /* init context for 2nd*/
        md5_update(context, k_opad, 64,buffer);     /* start with outer pad */
        md5_update(context, digest, 16,buffer);     /* then results of 1st */
        md5_finish(context,buffer,digest);
}