some testing code for SNMPV3 USM MD5

testhmac.c

/*
build with 
gcc -std=c99 testhmac.c -o testhmac -lssl -lcrypto
*/
#include <openssl/des.h>
#include <openssl/aes.h>
#include <openssl/sha.h>
#include <openssl/md5.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

typedef MD5_CTX               MD5HashStateType;
#define MD5_INIT(s)           MD5_Init(s)
#define MD5_PROCESS(s, p, l)  MD5_Update(s, p, l)
#define MD5_DONE(s, k)        MD5_Final(k, s)


int AuthMD5_password_to_key(const unsigned char *password,
                             const unsigned int   password_len,
                             const unsigned char *engine_id,
                             const unsigned int   engine_id_len,
                             unsigned char *key,
                             unsigned int *key_len)
{
  *key_len = 16; /* All MD5 keys have 16 bytes length */

#ifdef __DEBUG
  debugprintf(5,"password: %s.",
              OctetStr(password, password_len).get_printable());
  debugprintf(5,"engineID: %s.",
              OctetStr(engine_id, engine_id_len).get_printable());
#endif

  MD5HashStateType md5_hash_state;
  unsigned char  *cp, password_buf[65];
  unsigned long   password_index = 0;
  unsigned long   count = 0, i;

  MD5_INIT(&md5_hash_state);   /* initialize MD5 */

  /**********************************************/
  /* Use while loop until we've done 1 Megabyte */
  /**********************************************/
  while (count < 1048576) {
    cp = password_buf;
    for (i = 0; i < 64; i++) {
      /*************************************************/
      /* Take the next octet of the password, wrapping */
      /* to the beginning of the password as necessary.*/
      /*************************************************/
      *cp++ = password[password_index++ % password_len];
    }
    MD5_PROCESS(&md5_hash_state, password_buf, 64);
    count += 64;
  }
  MD5_DONE(&md5_hash_state, key);      /* tell MD5 we're done */

#ifdef __DEBUG
  debughexcprintf(21, "key", key, *key_len);
#endif

  /*****************************************************/
  /* Now localize the key with the engine_id and pass  */
  /* through MD5 to produce final key                  */
  /* May want to ensure that engine_id_len <= 32,      */
  /* otherwise need to use a buffer larger than 64     */
  /*****************************************************/
  memcpy(password_buf,                            key,       *key_len);
  memcpy(password_buf + *key_len,                 engine_id, engine_id_len);
  memcpy(password_buf + *key_len + engine_id_len, key,       *key_len);

  MD5_INIT(&md5_hash_state);
  MD5_PROCESS(&md5_hash_state, password_buf, (2 * *key_len) + engine_id_len);
  MD5_DONE(&md5_hash_state, key);

#ifdef __DEBUG
  debughexcprintf(21, "localized key", key, *key_len);
#endif

  return 1;
}


int AuthMD5_auth_out_msg(const unsigned char *key,
                          unsigned char *msg,
                          const int      msg_len,
                          unsigned char *auth_par_ptr)
{
  MD5HashStateType md5_hash_state;
  int           key_len = 16; /* We use only 16 Byte Key! */
  unsigned char digest[16];
  unsigned char k_ipad[65];   /* inner padding - key XORd with ipad */
  unsigned char k_opad[65];   /* outer padding - key XORd with opad */

  memset((char*)(auth_par_ptr), 0, 12);

#ifdef __DEBUG
  debughexcprintf(21, "key", key, 16);
#endif

  /*
   * the HMAC_MD5 transform looks like:
   *
   * MD5(K XOR opad, MD5(K XOR ipad, msg))
   *
   * where K is an n byte key
   * ipad is the byte 0x36 repeated 64 times
   * opad is the byte 0x5c repeated 64 times
   * and text is the data being protected
   */

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

  /* XOR key with ipad and opad values */
  for (int i=0; i<64; i++) {
    k_ipad[i] ^= 0x36;
    k_opad[i] ^= 0x5c;
  }

  /* perform inner MD5 */
  MD5_INIT(&md5_hash_state);            /* init md5_hash_state for 1st pass */
  MD5_PROCESS(&md5_hash_state, k_ipad, 64);    /* start with inner pad      */
  MD5_PROCESS(&md5_hash_state, msg, msg_len);  /* then text of datagram     */
  MD5_DONE(&md5_hash_state, digest);           /* finish up 1st pass        */
  /* perform outer MD5 */
  MD5_INIT(&md5_hash_state);            /* init md5_hash_state for 2nd pass */
  MD5_PROCESS(&md5_hash_state, k_opad, 64);    /* start with outer pad      */
  MD5_PROCESS(&md5_hash_state, digest, 16);    /* then results of 1st hash  */
  MD5_DONE(&md5_hash_state, digest);           /* finish up 2nd pass        */

#ifdef __DEBUG
  debughexcprintf(21, "digest", digest, 128 / 8);
#endif

  memcpy(auth_par_ptr, digest, 12);

  return 1;
}

static void dumpbuf( const unsigned char *buf, int len)
{
  for (int i=0; i< len; i++) {
		printf("%02X ", buf[i]);
	    if ((i+1)%4==0) printf(" ");
    	if ((i+1)%16==0) printf("\n");
  	}
	printf("\n");
}

int main( int argc, char *argv[])
{
	unsigned char keymd5[100], authpar[100];
	unsigned int keylen;
 	unsigned char engine_id[]={ 0x80, 0x00, 0x14, 0x55, 0x03, 0x00, 0x40, 0xc7, 0x1c, 0x70,0x22 };
	unsigned char msg[]= {  /*0x30, 0x2f, 0x04, 0x0b, 0x80, 0x00, 0x14, 0x55, 0x03, 0x00, 0x40, 0xc7, 0x1c, 0x70, 0x22, 0x04, 0x00, 
	0xa1, 0x1e, 0x02, 0x04, 0x74, 0xbb,*/ 0xbc, 0x2d, 0x02, 0x01, 0x00, 0x02, 0x01, 0x00, 0x30, 0x10, 0x30, 0x0e, 0x06, 0x0a, 0x2b, 0x06, 0x01, 0x02, 0x01, 0x01, 0x09, 0x01, 0x04, 0x01, 0x05, 0x00  };
	printf("sizeof engine_id = %d, sizeof msg = %d\n", sizeof(engine_id), sizeof(msg));
	AuthMD5_password_to_key( (unsigned char*)"testtest",8,
                     engine_id, sizeof(engine_id), keymd5, &keylen);
	dumpbuf( keymd5, keylen);

	AuthMD5_auth_out_msg( keymd5, msg, sizeof(msg), authpar); 
	dumpbuf( authpar, 12);
	return 0;
}